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Abstract 


Using  simple  passive  stereovision  pin-hole  sensors,  an  effective  reactive  collision  avoidance  al¬ 
gorithm  is  presented  in  this  report  for  unmanned  aerial  vehicle  (UAV).  Both  kinematic  model 
as  well  as  point  mass  models  of  UAV  are  considered  for  system  dynamics.  The  stereovision 
camera  is  considered  mounted  on  the  UAV.  The  camera  sensors  and  the  obstacle  are  forming 
a  geometrical  configuration  and  triangulation  methods  are  used  to  estimate  the  position  and 
velocity  of  the  obstacle  falling  under  the  field  of  view  of  the  camera  sensors.  For  simplicity, 
shape  of  the  obstacle  is  considered  as  spherical.  The  position  estimation  of  the  obstacle  is 
also  carried  out  by  image  processing  methods  wherein  the  feature  points  detection,  stereo 
matching  and  triangulation  algorithm  are  used  to  compute  the  3D  reconstruction  of  obstacle. 
This  method  uses  the  computer  vision  system  toolbox  and  VRML  viewer  of  MATLAB  @, 
Using  this  we  are  able  to  compute  the  approximate  size  and  centre  of  the  obstacle.  The 
camera  sensors  are  considered  noisy  and  the  noise  is  approximated  by  a  gaussian  random 
variable.  First,  an  Extended  Kalman  Filtering  (EKF)  approach  is  proposed  to  extract  the 
useful  information  from  the  noisy  information  generated.  Further  Unscented  Kalman  Fil¬ 
ter  (UKF)  approach  is  proposed  for  capturing  the  camera  sensor  nonlinearity  in  a  better 
manner  as  UKF  enables  a  second  order  approximation.  This  geometrical  formulation  takes 
advantage  of  both  ‘stereo  vision’  as  well  as  ‘optical  flow’  signatures  and  hence  it  is  capable  of 
estimating  the  range  information  as  well,  making  its  position  estimate  quite  accurate.  Fur¬ 
ther  the  obstacle  velocity  information  is  also  estimated  based  on  optical  flow  information. 
Next,  an  ‘aiming  point’  is  computed  after  putting  an  artificial  safety  ball  around  the  obsta¬ 
cle  and  using  the  collision  cone  approach.  Next,  the  velocity  vector  of  the  vehicle  is  steered 
away  towards  this  aiming  point  using  a  recently  developed  ‘differential  geometry  guidance’ 
which  is  a  dynamic  inversion  based  three-dimensional  nonlinear  aiming  point  guidance  law. 
The  guidance  command  generation  is  based  on  angular  correction  between  the  actual  and 
the  desired  direction  of  the  velocity  vector.  Note  that  the  velocity  vector  gets  aligned  along 
the  selected  aiming  point  quickly  (i.e.  within  a  fraction  of  the  available  time-to-go),  which 
makes  it  possible  to  avoid  pop-up  obstacles.  A  large  number  of  simulation  studies,  which 
also  includes  consistency  checks  for  filtering  algorithms,  leads  to  the  conclusion  that  this 
strategy  is  quite  effective  in  avoiding  popup  obstacles  within  a  very  short  time  and  hence 


can  be  very  useful  for  reactive  collision  avoidance.  The  guidance  algorithm  has  been  verified 
with  simulations  carried  out  both  for  single  obstacles  as  well  as  for  multiple  obstacles  on 
the  path,  stationary  as  well  as  moving  obstacles  and  also  with  different  safety  sphere  sizes 
around  the  obstacles.  The  proposed  algorithm  has  been  validated  using  both  kinematic  as 
well  as  point  mass  model  of  a  prototype  unmanned  aerial  vehicle.  For  better  confidence, 
results  have  also  been  validated  by  incorporating  a  first  order  autopilot  delay  compensation 
for  control  commands. 
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Chapter  1 
Introduction 


UAV’s  are  used  for  many  missions  of  practical  importance  due  to  their  ability  to  fly  au¬ 
tonomously  at  low  altitudes.  The  UAVs  can  be  deployed  in  numerous  fields.  The  major 
areas  of  applications  are  reconnaissance  and  surveillance,  targeted  attacks  with  less  collat¬ 
eral  damage,  battle  damage  assessment,  traffic  monitoring  for  crime  prevention,  detection 
and  containment  of  hazardous  leakages  in  industries,  assessment  and  rehabilitation  in  case 
of  natural  calamities,  military  and  terrain  mapping  etc.  While  operating  at  low  altitude, 
the  UAV  may  come  across  many  obstacles  on  it’s  path,  some  of  them  are  well  known  such 
as  urban  structures,  buildings,  trees  etc.  Other  obstacles  may  suddenly  appear  as  pop-up 
threat  across  it’s  path.  The  UAV’s  are  generally  equipped  with  path  planning  algorithms 
that  take  care  of  all  known  obstacles  and  accordingly  the  trajectory  up  to  destination  is  well 
planned.  The  pop-up  threats  are  unpredictable,  therefore  all  possible  collision  avoidances 
cannot  be  accounted  a-priori,  hence,  it  is  vital  that  UAV’s  should  have  the  capability  to 
cater  for  all  kinds  of  collision  avoidance  with  obstacles  autonomously  and  safely  reach  to  it’s 
destination.  A  variety  of  path  planning  algorithms  are  available  which  are  used  to  design 
the  trajectory  of  a  UAV.  Generally  these  algorithms  are  computed  off-line  and  are  stored 
in  UAV  onboard  processor.  However,  there  is  no  such  off-line  mechanism  for  guiding  the 
UAV  in  a  scenario  when  the  obstacles  suddenly  appear  as  a  pop-up  threat  across  the  UAV 
trajectory.  Therefore,  there  is  an  immense  need  of  an  online  algorithm  that  is  built-in  the 
processor  to  compute  the  alternative  path  for  the  UAV  satisfying  it’s  own  constraints  and 
safely  protect  it  from  colliding  in  a  given  minimum  available  time.  The  online  algorithm 
that  is  computationally  fast,  highly  efficient  and  noninteractive  in  nature  serves  such  kind 


1 


of  purpose  and  are  called  as  reactive  collision  avoidance  algorithm.  The  trajectory  described 
in  the  Fig.  1.1  shows  the  reactive  collision  avoidance  for  the  pop-up  threat. 

The  collision  avoidance  problem  addressed  in  this  report  can  in  fact  be  broadly  termed 
into  the  problem  of  ’path  planning’,  which  is  the  process  of  hireling  a  safe  flight  path  to  the 
goal  point.  It  typically  consists  of  two  layers  (i)  a  global  path  planner  and  (ii)  a  local  path 
planner.  A  global  path  planning  algorithm  accounts  for  all  known  obstacles  in  the  domain 
a-priori  and  plans  a  path  to  the  destination  in  such  a  way  that  it  avoids  all  obstacles  on  its 
path.  Some  global  path  planning  algorithms  such  as  rapidly  exploring  random  tree  (RRT), 
Potential  held  method  and  graph  search  methods  have  been  proposed  in  the  literature  for 
this  task.  An  interested  reader  can  see  [6]  for  an  overview  of  these  methods.  This  path 
planning  is  deterministic  in  nature.  For  local  path  planning,  however,  the  main  aim  is  to 
avoid  collisions  with  obstacles  at  close  vicinity  (especially  with  the  pop-up  threats),  which 
is  done  in  the  following  manner.  When  an  onboard  sensor  detects  obstacle (s)  and  predicts  a 
possible  collision,  the  guidance  law  attempts  to  maneuver  the  vehicle  away  from  the  danger 
as  soon  as  possible  so  that  the  impending  collision  is  averted.  Such  a  guidance  is  also  called 
as  ’reactive  guidance’,  since  the  available  reaction  time  is  very  small  and  decisions  have  to 
be  taken  quickly  (i.e.  within  a  fraction  of  the  available  time-to-go).  Note  that  reactive 
collision  avoidance  guidance  typically  results  in  high  manoeuvres  for  a  small  duration  of 
time.  Moreover,  the  guidance  law  should  also  ensure  that  while  avoiding  obstacles  the 
vehicle  should  not  deviate  too  much  from  its  original  intended  path.  This  is  both  because 
other  obstacles  should  not  come  on  its  new  flight  path  and  it  should  not  deviate  too  much 
away  from  seeking  its  intended  destination  (otherwise,  putting  it  back  into  track  becomes 
difficult).  Another  critical  restriction  on  such  a  reactive  guidance  law  is  that  it  should  be 
computationally  quite  efficient  (preferably  available  in  closed  form),  since  the  time  to  react 
is  typically  quite  low  and,  if  possible,  the  control  and  guidance  update  needs  to  be  done 
at  a  higher  rate.  Moreover,  for  fixed-wing  UAVs,  it  should  also  assure  that  the  vehicle 
keeps  moving  at  sufficiently  higher  velocity  to  avoid  stalling.  In  view  of  these,  there  is  a 
need  for  developing  a  reactive  guidance  scheme  based  on  sound  geometric  and  mathematical 
considerations.  Preferably  it  should  also  have  sufficient  generality  so  that  it  can  be  applied 
for  a  wide  range  of  applications. 

It  is  vital  that  UAV  should  fly  autonomously  to  sense  and  avoid  collisions  [38].  Environ¬ 
ment  sensing  is  generally  achieved  through  aboard  passive  vision  sensing  through  onboard 
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Figure  1.1:  Obstacle  Collision  Avoidance 

cameras.  It  has  been  widely  implemented  due  to  their  light  weight,  low  cost  and  energy 
usage [11].  When  obstacles  are  sensed  in  the  environment,  UAV  must  be  able  to  react  and 
maneuver  quickly  so  that  the  collision  is  averted.  Because  of  the  fact  that  the  time  avail¬ 
ability  is  small,  the  collision  avoidance  guidance  algorithm  should  also  be  computationally 
efficient  (preferably  should  be  computed  in  closed  form).  To  achieve  this,  an  algorithm  needs 
to  be  designed  which  steers  the  vehicle  to  avert  the  obstacle  as  well  as  plan  the  vehicle’s 
path  as  fast  enough  to  be  implemented  online.  Such  algorithms  are  called  “reactive  obstacle 
avoidance  algorithm” .  These  algorithm  may  sustain  the  challenge  of  heavy  computational 
limit  imposed  by  UAV  flying  at  high  speeds.  Apart  from  speed,  an  important  requirement  is 
low  computational  resource  usage,  so  that  it  may  be  suitable  for  onboard  execution.  Many 
global  path  planning  algorithms  are  computationally  expensive,  hence  they  are  inadequate 
to  be  solved  in  the  limited  memory  usage  [12].  Hence,  there  is  a  urgent  need  of  new  technique 
which  is  preferably  based  on  sound  geometric  and  mathematical  considerations  so  that,  due 
its  generality  and  scalability,  it  can  be  applied  for  wide  range  of  applications. 

In  this  report,  a  reactive  collision-avoidance  algorithm  is  proposed,  the  UAV  invokes 
the  reactive  collision-avoidance  algorithm  only  when  an  obstacle  is  encountered  by  onboard 
stereovision  sensors  across  its  trajectory.  The  collision  avoidance  occurs  in  two  steps;  first, 
the  collision  is  detected  based  on  the  collision  cone  approach  [41,  17]  using  vision  sensor  data. 
Next,  if  the  threat  is  observed,  a  collision  avoidance  maneuver  is  performed  with  respect  to 
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an  ’aiming  point’ [34],  judiciously  selected  on  the  obstacle’s  safety  sphere  surface.  Note  that 
the  concept  can  be  interpreted  in  the  light  of  ’pursuit  guidance’,  where  the  objective  is  to 
reorient  or  align  the  velocity  vector  of  the  UAV  to  the  line-of-sight  [33].  In  all  the  cases,  the 
gains  are  selected  such  that  the  obstacles  are  avoided  quickly  (i.e.  within  a  fraction  of  the 
available  time-to-go). 

Vision  sensors  are  finding  its  usefulness  as  an  aid  for  navigation  and  guidance  systems  for 
UAVs  in  many  potential  operations.  Since  such  sensors  are  usually  cheap,  light-weight,  fairly 
reliable  and  more  importantly  they  are  passive  in  nature.  A  passive  sensor  is  not  susceptible 
to  signal  jamming  either.  It  can  also  enable  the  vehicle  in  GPS  denied  environments  as  well 
as  in  the  areas  where  the  navigation  information  is  not  available  on  the  trajectory  of  UAV. 
However,  there  are  several  drawbacks  as  well,  which  are  the  noisy  nature  of  the  signal,  the 
lack  of  range  information  and  poor  resolution.  However,  a  single  passive  vision  sensor  is 
only  capable  to  provide  direction  information  of  the  obstacle  in  general.  Unless  the  relative 
velocity  between  the  vehicle  and  obstacle  is  high,  the  range  signature  is  very  weak  (it  is 
zero  if  there  is  no  relative  velocity)  and  hence  it  can  not  be  estimated  well.  To  address  this 
drawback,  a  stereo  vision  sensor  based  formulation  is  presented  in  this  report,  where  the 
geometric  constraint  serves  as  a  ‘virtual  sensing’  of  the  range. 

Vision  sensors  capture  the  projection  of  the  obstacle  environment  on  their  image  plane 
and  the  data  frames  obtained  by  the  vision  sensor  is  processed  by  onboard  image  processor 
to  find  the  information  about  the  obstacle.  The  practical  stereo  camera  models  available 
are  noisy  in  nature.  Therefore  information  gathered  by  the  Vision  sensor  is  not  accurate. 
Which  if  used  directly  can  give  erroneous  results.  In  order  to  deal  with  such  issues  filtering 
algorithms  such  as  an  Extended  Kalman  Filter  (EKF)  [5,  3]  and  Unscented  Kalman  Filter 
[3,  4]  are  used  which  tries  to  extract  the  useful  information  from  the  noisy  data. 

A  local  path  planning  algorithm  attempts  to  avoid  unforeseen  obstacles  which  suddenly 
appear  on  its  flight  path  in  close  proximity.  The  time-to-go  in  such  problems  is  usually  small 
and  the  vehicle  has  to  take  quick  corrective  action  of  its  flight  path  within  the  available 
short  time.  It  is  often  called  as  ’reactive  collision  avoidance’.  Since  the  onboard  processor 
installed  in  UAVs  have  a  very  limited  computing  capability,  such  an  algorithm  needs  to 
be  computationally  very  efficient.  Using  an  innovative  ‘collision  cone’  approach,  such  an 
algorithm  was  first  proposed  in  [41]  and  the  philosophy  has  subsequently  been  augmented 
with  3D  prospective  [17].  Using  this  idea  and  fusing  it  with  the  ‘aiming  point  guidance’  [8],  a 
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‘differential  geometry  guidance’  algorithm  has  been  proposed  recently  in  [7],  which  has  been 
used  in  this  report  as  well.  Note  that  such  an  approach  has  also  been  proposed  earlier  in  [9], 
but  it  was  only  with  a  single  camera  formulation  and  hence  was  only  moderately  successful. 
The  approach  presented  here,  however,  involved  a  two  camera  formulation  and  hence  turns 
out  to  be  much  more  effective  because  of  better  estimation  of  the  obstacle  position  [10]  and 
velocity.  A  large  number  of  simulation  studies,  which  include  cr-bound  consistency  checks  of 
EKF  as  well  as  UKF,  clearly  brings  out  this  fact. 

Simulation  studies  have  been  carried  out  with  the  ’Kinematic  model’  as  well  as  the 
’Point  mass  model’  of  the  real  fixed  wing  UAV  to  test  the  performance  of  the  nonlinear 
differential  geometric  guidance  scheme.  Various  cases  have  been  considered  with  one  and 
multi-obstacle  scenario,  different  safety  radius  of  the  obstacle,  stationary  as  well  as  moving 
obstacles  in  noise-free  and  noisy  conditions.  For  all  the  cases,  the  UAV  avoids  the  obstacle 
and  reaches  the  goal  point.  Results  are  also  validated  by  incorporating  first  order  autopilots 
for  the  guidance  commands  in  case  of  point  mass  dynamics  of  UAV. In  the  present  work, 
the  problem  of  reactive  obstacle  avoidance  for  UAV  is  addressed  by  collision  cone  approach 
using  differential  geometric  guidance  and  forming  a  geometrical  configuration  with  a  stereo 
camera  model. 


1.1  Motivation 

The  problem  of  reactive  collision  avoidance  for  UAV  has  been  heavily  researched  in  recent 
literature.  The  artificial  potential  field  method  [13]  is  a  popular  approach  due  to  its  intuitive 
nature  and  capability  to  be  tailored  to  different  types  of  problems.  The  potential  fields  in 
obstacle  avoidance  are  tailored  such  that  obstacles  have  a  repulsive  field  while  the  destination 
has  an  attractive  field.  The  resultant  field  represents  a  safe  direction  for  the  UAV  to  move 
along.  However,  this  algorithm  is  not  strictly  reactive,  since  at  every  instant  it  takes  into 
account  the  presence  of  all  (or  most  of)  the  obstacles  in  the  environment  before  deciding  the 
direction.  A  model-predictive  control  (MPC)  based  collision  avoidance  algorithm  is  proposed 
[14] ,  in  which  a  potential  field  function  is  incorporated  in  the  cost  function  to  be  minimized. 
The  other  terms  in  the  cost  function  include  costs  for  path  following,  control  saturation 
and  input  saturation.  The  advantage  of  using  MPC  is  that  state  and  input  constraints  are 
accounted.  The  disadvantage  of  such  a  strategy  is  that  the  algorithm  functions  under  rigid 
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path  following  requirements  and  does  not  actively  seek  the  destination.  Further,  MPC  is  a 
resource-intensive  algorithm  that  requires  a  powerful  processor,  making  the  method  unsuit¬ 
able  for  implementation  aboard  a  UAV.  Another  approach  in  reactive  collision  avoidance 
is  RRT  [12],  which  is  a  randomized  search  algorithm.  In  RRT  algorithm  the  length  of  the 
path  found  is  far  from  optimal  and  may  have  several  extraneous  branches  due  to  the  random 
nature  of  the  algorithm.  Although  reactive  collision  avoidance  permits  maneuvers  that  are 
not  optimal  but  the  wastage  in  the  path  found  by  RRT  is  significant.  However,  a  path  prun¬ 
ing  algorithm  can  refine  the  path  but  such  a  step  is  infeasible  for  online  collision  avoidance. 
Some  graph  search  algorithms  like  the  best-first  search  algorithm  are  implemented  for  reac¬ 
tive  collision  avoidance  [15].  In  the  best-first  search  method  a  sorted  list  of  pre-computed 
motion  primitives  are  created.  Saving  the  pre-computing  motion  primitives  in  a  lookup  table 
is  infeasible  for  UAV  applications  due  to  the  large  memory  resources  demanded. 

Even  the  problem  of  UAV  pursuing  its  goal  is  also  a  similar  approach,  which  has  been 
implemented  with  intermediate  obstacles  in  the  path  using  PN  guidance  based  collision 
avoidance  scheme  [16].  However,  this  scheme  leads  to  a  jump  in  the  control  effort  every  time 
a  new  target  is  pursued.  Instead,  a  minimum  effort  guidance  (MEG)  approach  minimizes  the 
control  effort  for  the  entire  trajectory  along  with  avoiding  collisions  for  multiple  targets  [17]. 
A  collision  cone  approach  [41]  is  used  to  detect  potential  collisions  by  considering  a  threat 
boundary  around  the  obstacle  in  MEG  guidance.  It  has  been  demonstrated  that  MEG  is 
more  suitable  than  PN  [17].  However,  collision  avoidance  problems  do  not  have  minimum 
effort  requirements  and  emphasize  vehicle  safety  over  low  control  effort.  The  MEG  guidance 
causes  the  vehicle  to  maneuver  until  the  point  of  impact,  which  is  risky.  Above  all,  the 
collision  avoidance  algorithm  must  ensure  that  the  UAV  full  dynamics  should  be  accounted. 
In  some  of  the  literature,  obstacle  avoidance  issues  are  addressed  through  the  kinematic 
model,  [18]- [19]  in  which  the  autopilot  responses  are  approximated  by  first  order  models. 
Even  3  —  DOF  motion  is  also  considered  to  some  extent  [17],  [20].  This  may  cause  vehicle 
to  take  large  and  practically  infeasible  maneuvers,  leading  to  state  or  control  saturation. 

1.2  Contribution 

In  the  present  work,  a  new  method  of  computing  the  aiming  point  is  developed.  The  philos¬ 
ophy  is  based  on  collision  cone  approach.  The  aiming  point  is  the  point  where  the  vehicle 
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has  to  quickly  maneuver  in  order  to  avoid  an  imminent  collision.  The  philosophy  behind 
this  approach  is  that  the  velocity  vector  is  projected  to  a  plane  which  intersects  the  obstacle 
safety  sphere  if  the  point  of  projection  lies  inside  the  obstacle  safety  sphere  then  the  aiming 
point  guidance  is  invoked  and  a  new  aiming  point  is  computed  which  demands  deflection  in 
UAV  velocity  so  that  collision  can  be  averted.  The  differential  geometric  guidance  is  used 
to  compute  the  guidance  command  for  driving  the  UAV  to  aiming  point  as  well  as  to  the 
destination. 

In  the  present  work,  a  reactive  obstacle  avoidance  algorithm  is  designed  which  has  been 
realized  with  kinematic  model  as  well  as  point  mass  model  of  a  realistic  UAV.  The  nonlin¬ 
ear  differential  geometric  guidance  (DGG)  algorithm  is  First,  successfully  implemented  with 
kinematic  model  and  Next,  validated  with  the  point  mass  model  based  formulation  and  sim¬ 
ulations  are  carried  out  with  randomized  input  conditions.  The  guidance  algorithm  detects 
the  obstacle  based  on  the  3 D  collision  cone  approach  [17]  and  the  avoidance  maneuver  is 
performed.  The  nonlinear  guidance  algorithm  generates  angular  commands  in  the  horizontal 
and  the  vertical  plane.  These  commands  are  then  pursued  by  the  UAV  to  reach  the  aiming 
point  [33],  [34],  The  aiming  point  is  the  point  of  contact  of  the  tangent  drawn  through  the 
UAV  location  to  the  safety  ball  skirting  the  obstacle.  The  tangent  is  the  line  of  sight  of 
the  vehicle  to  the  aiming  point.  The  concept  is  implemented  in  the  direction  of  the  pursuit 
guidance  [33]  where  the  objective  is  to  reorient  the  velocity  vector  of  the  vehicle  to  the  line 
of  sight  (LOS)  within  the  fraction  of  the  available  time-to-go.  It  finally  steers  UAV  towards 
the  aiming  point  and  hence  averts  the  obstacle.  Pursuit  guidance/aiming  point  guidance 
[33], [34]  philosophy  is  used  in  the  missile  guidance  to  aim  at  the  predicted  position  of  the 
target  at  the  final  time. 

The  algorithm  is  successfully  realized  with  noise-free  sensor  as  well  as  plant  model  of 
UAV.  In  order  to  test  the  performance  in  noisy  conditions,  simulations  are  carried  out  with 
noisy  sensor  as  well  as  plant  model.  The  estimation  techniques  are  used  to  extract  the  useful 
information  from  the  noisy  data.  The  reactive  collision  avoidance  algorithm  is  successfully 
realized  under  noisy  conditions. 

In  the  present  work,  a  recently  developed  Nonlinear  differential  geometric  guidance 
(DGG)  is  used  for  reactive  collision  avoidance.  The  obstacles  considered  in  the  problem 
are  moving  as  well  as  stationary.  The  geometric  configuration  is  formed  with  stereovision 
pin  hole  camera  sensors  mounted  on  the  UAV.  The  camera  sensors  capture  the  projection 
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of  the  obstacle  on  image  plane.  Using  the  camera  coordinate  information  of  the  pixel  as 
well  as  the  sequential  pixel  frames,  one  can  compute  the  obstacle  position  as  well  as  obstacle 
velocity  within  prescribed  tolerance  bounds. 

Simulation  studies  have  been  carried  out  with  different  plant  models  to  test  the  per¬ 
formance  of  the  innovative  nonlinear  reactive  guidance  scheme  elaborately.  Scenarios  with 
different  number  and  size  of  the  obstacles  in  the  environment  have  been  considered.  Point 
mass  model  with  a  coordinated  flight  is  demonstrated  with  all  scenarios,  where  the  controller 
dynamics  is  approximated  by  the  first  order  autopilots  [20]. 

Various  simulation  studies  clearly  show  that,  using  the  stereo  camera  sensors  in  the  spe¬ 
cific  geometrical  formulation  and  using  the  new  methodology  of  the  aiming  point  computa¬ 
tion,  the  reactive  collision  avoidance  for  stationary  as  well  as  moving  obstacles  is  performed 
for  the  UAV  model.  The  aiming  point  is  achieved  by  applying  the  nonlinear  differential 
geometric  guidance  law,  which  maneuvers  the  UAV  quickly  in  available  time-to-go.  The 
technique  is  quite  effective  in  avoiding  collisions  in  different  scenarios.  In  all  the  simulations, 
all  the  constraints  posed  by  the  vehicle  capability  are  very  well  met  within  the  available 


Chapter  2 

Obstacle  Position  and  Velocity 
Estimation 


In  this  section,  stereo  geometry  formulation  with  two  pin-hole  cameras  and  an  obstacle, 
falling  under  field  of  view  (fov)  of  both  cameras,  has  been  discussed.  Here,  the  objective 
is  to  compute  the  position  of  the  obstacle  using  the  geometry  formulation.  The  obstacle  is 
considered  as  a  point.  The  projection  of  this  point  obstacle  on  the  camera  image  planes  are 
processed  through  the  onboard  image  processors  and  the  corresponding  camera  coordinates 
are  assumed  to  be  known.  The  position  of  the  obstacle  can  be  obtained  using  triangulation 
method  using  the  data  obtained  from  camera  sensors.  However,  the  vision  sensors  are 
assumed  to  be  noisy.  So,  we  need  to  estimate  exact  position  of  the  obstacle  using  filtering 
technique  such  as  Extended  kalman  filter(EKF)  and  Unscented  kalman  filter(UKF)  which 
are  discussed  in  later  part  of  this  section.  Due  to  the  sensor  noise,  there  is  uncertainty  in 
exact  position  estimation.  Therefore,  the  safety  of  the  point  obstacle  is  insured  by  assuming 
the  obstacle  at  the  center  of  an  sphere  and  the  sphere  is  treated  as  virtual  obstacle  and 
calling  the  radius  of  sphere  as  safety  radius  of  the  point  obstacle.  Therefore,  the  sphere 
with  finite  safety  radius  is  eventually  termed  as  obstacle  with  safety  bounds.  The  center 
and  radius  of  this  spherical  obstacle  can  be  computed  by  image  processing  techniques  by 
capturing  the  spherical  obstacle  image  through  camera  vision  sensors. 
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2.1  Stereo  Geometry 

The  geometrical  formulation  [10]  for  computing  the  position  of  the  obstacle  with  stereo 
vision  sensors  is  considered  as  per  Fig.  2.1. 


Left 


Figure  2.1:  Geometry  Formulation  with  Two  Pin-hole  Camera 

The  pin-hole  cameras  ( PCr  and  PC\)  are  fitted  on  the  UAV  are  symmetric  with  respect 
to  longitudinal  axis  as  well  as  center  of  the  gravity  O.  Both  camera’s  optical  axis  are  parallel 
and  separated  by  baseline  distance  B.  The  focal  length  for  both  the  cameras  is  same  and  is 
denoted  by  /.  Cameras  are  assumed  to  be  non- rotating.  The  y  axis  of  the  3D  coordinate 
system  is  parallel  to  the  baseline.  The  obstacle  is  located  at  point  (x,  y,  z )  should  be  visible 
to  both  the  cameras  for  range  information  extraction.  The  range  computation  of  the  obstacle 
is  determined  based  on  the  baseline  separation,  which  should  be  more  for  faraway  obstacle. 
The  image  coordinates  corresponding  to  the  obstacle  for  left  camera  is  (yh  z\)  and  for  the 
right  camera  is  (y'r,zr).  The  range  information  of  the  obstacle  is  computed  from  the  stereo 
images  by  obtaining  (61/,  </>/)  from  left  camera  and  (6r  ,  <f>r)  from  right  camera.  Where  9i,  9r 
are  the  azimuth  angle  and  (pi,  0r  are  the  elevation  angle  of  obstacle  on  the  image  plane  of 
both  cameras. 
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Using  the  triangle  rule  for  camera  coordinate  frame 

y'i  =  f  tan(9i ) 

(2.1) 

y'r  =  f  tan(9r) 

(2.2) 

z'i  =\/[/2  +  (%')2]  tan(^) 

(2.3) 

U-  =V[/2  +  (l/r)2] 

(2.4) 

Using  similar  triangles, 

y,'  _  H/  +  !) 

-/  a; 

(2.5) 

y'r  ..  C-y-f) 

-/  ® 

(2.6) 

-/  -/  a: 

(2.7) 

The  obstacle  position  can  be  computed  with  simple  algebra  on  Eq.(2.5)  to  Eq.  (2.7)  as 

Bf 

x=  (  , 

\Vl  ~  Vr) 

(2.8) 

Biyl  +  y'r) 

2  {y'l-y'r) 

(2.9) 

B{z[  +  z'r) 

^y'l-y'r) 

(2.10) 

Thus  the  obstacle  position  is  a  function  of  disparity  which  is 

defined  as  {y\  —  y'r). 
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2.2  Obstacle  Position  Estimation  using  EKF 


Position  information  of  the  obstacle  is  computed  with  sensor  data  obtained  from  vision 
sensing  cameras.  The  vision  sensors  are  assumed  to  be  noisy.  Moreover,  the  UAV  model  is 
considered  with  process  noise  as  well.  To  address  these  issues,  EKF  [3]  is  used  to  filter  out 
the  noise  and  estimate  the  obstacle  position  accurately.  An  assumption  made  here  is  that 
the  measurement  noise  uncertainty  is  within  10%  of  the  actual  value. 

2.2.1  Dynamics  of  System 

The  camera  sensor’s  nonlinear  state  dynamics  as  per  state  space  model  is  given  as  follows 


Xr  =  f(Xr,t)  +  G(t)w(t)  (2.11) 

Tfc  =  h(Xr(k))  +  z/fc  (2.12) 

where  camera  sensor  state  propagation  equation  is  in  continuous  time  domain  and  output 
state  measurement  equation  is  in  discrete  time  domain,  where  f(Xr,  t )  is  nonlinear  transition 
matrix  function  and  h{Xr{k ))  is  nonlinear  measurement  matrix  function.  G(t)  is  process 
noise  influence  matrix,  it  is  considered  as  G(t)  =  I.  Process  noise  w(t)  and  measurement 
noise  uk  are  independent,  zero  mean,  gaussian  noise  processes  with  their  covariance  matrix 
Q  and  R  respectively.  The  mathematical  expression  is  as  follows 

E[w(t)wT(r)]  =  Q{t)5(t  —  t)  (2-13) 

E[vkvJ]  =  RSkj  (2.14) 

where  S(t  —  r)  is  dirac-delta  function  and  5k j  is  kronecker  delta  function. 

In  EKF  framework,  linearize  the  nonlinear  state  space  model  at  each  instant  around  the 
current  state  estimate,  therefore  f(Xr,t )  and  h(Xr(k ))  are  linearized  around  Xr.  Use  the 
linearized  model  for  state  error  covariance  propagation.  The  state  vector  derivative  Xr  is 
defined  as 


Xr 


fi  0i 


4*r 


T 
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where  r,,  9i  and  0/  are  left  camera  states  and  r,,  9r  and  0r  are  right  camera  states.  The 
dynamics  of  camera  state  vector  is  given  as 


n 

cos  9i  cos  <j>iUr  +  sin  0,  cos  0py  +  sin  0,uy 

9i 

sin\ur+  cos\vr 

rt  cos  (pi  7  rL  cos  <pt  r 

<i>i 

cos  0i  sin  4>i  ^  sin  9i  sin  (j>i  ,  cos  fa  w 

n  r  ri  r  ri  r 

fr 

cos  9r  cos  0,u,  +  sin  9r  cos  0,n,  +  sin  0ruy 

9r 

sin9\ur+  cos\vr 

rr  cos  (pr  '  rr  cos  cpr  ' 

0, 

cos  0r  sin  (pr  sin  0r  sin  $r  .  cos  <pr 

r  dir  r  dJr  \  Wr 

where,  ur,  vr  and  wr  are  relative  velocity  components  of  UAV  along  x,  y  and  z  directions. 


2.2.2  Camera  Sensor  Measurement  Noise  Model 


Camera  Sensor  measurement  noise  model  is  considered  based  on  the  fact  that  noise  is  pro¬ 
portional  to  sensing  range  as  the  sensing  range  increases  uncertainty  also  increases.  There¬ 
fore,  measurement  noise  model  is  considered  as  per  Eq.(2.16)  where  rnk  is  percentage  noise 
at  time  instant  k,  m 0  is  initial  percentage  measurement  noise  taken  as  20,  r(k)  is  the  range 
of  obstacle  at  time  instant  k  and  5  is  a  tuning  parameter  considered  as  0.99  which  defines 
how  rnk  changes  with  r(k) 

mk  =  mo  (l  —  (2-16) 

Measurement  noise  covariance  can  be  computed  based  on  rrik  given  as 


Mk  = 


Wy  I 

mk -  x  - 

100  3 


(2.17) 


where  wv  is  the  angular  width  of  the  camera  held  of  view.  It  is  assumed  120°  for  both 
horizontal  and  vertical  axis. 


2.2.3  Initialization 


The  camera  state  vector  is  initialized  with  first  measurement  of  vision  sensors 

*r(0)=[r,(0)  0,(0)  <M°)  ry(0)  0r(O)  0,(0)  ]T  (2.18) 

The  process  noise  covariance  matrix  Q  is  initialize  as 

Q  =  diag{  0.25,  0.028,  0.028,  0.25,  0.028,  0.028  )  (2.19) 
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The  diagonal  elements  of  Q  corresponds  to  range  r(m),  angles  9 (radian )  and  <f)(radian ) 
of  left  and  right  cameras  respectively.  The  error  covariance  matrix  is  initialized  as 

P0  =  diag  (  pi,  )  (2.20) 

where, 

Po  =  drag  (  aiel\  a2(^)2,  a3(^)2  )  (2.21) 

Pq  =  diag  (  cqer2,  a2(«)2,  a3(^)2  )  (2.22) 

where  P3  and  P0r  are  the  error  covariance  matrix  and  el  and  er  are  the  corresponding 
initial  error  in  the  range  of  left  and  right  camera  respectively,  eq  =  1,  a2  =  2  and  a3  =  2  are 
tuning  parameters. 

2.2.4  Extended  Kalman  Filter 

In  EKF  design,  vision  sensor  model  is  nonlinear  and  the  kalman  filter  is  extended  through 
linearization  of  nonlinear  dynamics.  The  camera  sensor  states  are  estimated  using  lineariza¬ 
tion  and  applying  extended  kalman  filter.  In  order  to  reduce  errors  due  to  linearization,  the 
sensor  true  state  should  be  close  to  estimated  state  at  all  time.  Therefore  the  error  dynamics 
can  be  represented  by  the  linearized  system  about  sensor’s  estimated  state. 

EKF  is  nonlinear  recursive  estimator.  It  linearizes  the  nonlinear  dynamics  around 
nominal  operating  point  and  operates  around  the  narrow  zone  across  the  nominal  operating 
point.  The  solution  is  recursive  in  the  sense  that  each  updated  estimate  of  state  is  computed 
from  previous  state  estimate  and  new  measurement  input  data.  Therefore  only  previous 
estimate  of  the  state  is  needed  and  entire  past  state  estimates  need  not  be  stored.  Kalman 
filter  is  computationally  more  efficient  than  directly  computing  the  state  estimate  from  entire 
past  observed  data  at  each  step  of  filtering  process. 

After  linearization  we  first  initialize  the  state  estimate  as  well  as  state  covariance  matrix 
and  then  propagate  the  state  estimation  vector  as  X+(k  —  1)  — »  X~(k)\  The  following 
equation  shows  the  state  propagation  which  is  as  per  state  dynamics. 

ir  =  f(Xr,t)  (2.23) 

and  propagate  the  covariance  matrix  P^_1  — »  P (T : 
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The  following  equation  shows  the  state  covariance  propagation  using  the  A  matrix  which 
is  also  known  as  system  matrix  of  the  linearized  sensor  state  equation. 

Pit)  =  A(t)P(t)  +  P(t)AT(t)  +  Q  (2.24) 

The  A  matrix  is  computed  by  Linearizing  Eq.(2.15).  The  mathematical  expression  for  A  is 
as  follows 


A{t)  ~  dxJUt)  (2'25) 

From  Eq.(2.17),  compute  measurement  error  covariance  matrix  R k  is  computed  based  on 
measurement  noise  model  considered 

Rk  =  diag(  (Mk)dl  (Mfc)*,  (. Mk)gr  (Mfc)0r  )  (2.26) 

Compute  Kalman  gain  Kk 

Kk  =  P;Cl\CkP;CTk  +  ft]"1  (2.27) 

Linearizing  h(Xr(k))  the  matrix  Ck  is  obtained  ,  which  is  the  measurement  transition 
matrix.  The  Ck  matrix  is  constant  therefore  it  is  not  required  to  compute  it  for  each  iteration 
and  saves  computation  time.  It  is  used  for  computation  of  sensor  output  as  described  in 
Eq.(2.29). 


0  0 
0  0 
1  0 
0  1 

Take  the  measurement  Yk  as 

k k  CJkXr  vk 


Ck  = 


dh 


0  10  0 
0  0  10 
0  0  0  0 
0  0  0  0 


(2.28) 


(2.29) 


As  the  measurement  from  the  camera  sensor  Yk  is  obtained,  update  the  camera  sensor 
state  vector  as  per  following  equation.  This  is  called  correction  step  for  state  estimate. 


xp(k)  =  x;{k)  +  Kk[Yk  -  h{X-(k))] 


(2.30) 
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Update  the  covariance  with  linear  covariance  dynamics.  This  is  also  called  correction 
step  for  state  error  covariance. 

P+  =  (I-  KkCk)P-  (/  -  KkCk)T  +  KkRkKk  (2.31) 

This  updated  camera  state  and  covariance  are  used  as  input  in  the  next  iteration  and  thus 
extended  kalman  filter  estimates  the  camera  states  within  a  reasonable  tolerance  bounds. 
However  Q  and  R  are  tuning  parameters  and  therefore  precaution  should  be  taken  while 
selecting  the  entries  in  these  matrices.  EKF  pre-run  is  carried  out  before  its  actual  applica¬ 
tion.  It  is  required  to  stabilize  the  initial  error  and  during  this  period  the  system  dynamics 
is  allowed  to  propagate  without  any  command. 

2.3  Unscented  Kalman  Filter 

Extended  kalman  hlter(EKF)  described  in  previous  section  is  used  for  recursive  nonlinear 
estimation.  EKF  however,  provides  only  first  order  approximation  to  nonlinear  estimation. 
Another  filter  known  as  Unscented  kalman  hlter(UKF)  has  better  performance  than  EKF. 
In  EKF  state  distribution  is  approximated  by  a  Gaussian  random  variable  and  the  state  is 
propagated  through  Erst  order  approximation  of  the  nonlinear  system.  However,  for  UKF 
the  state  distribution  is  approximated  by  a  gaussian  random  variable,  but  state  distribution 
is  represented  using  a  minimal  set  of  sample  points.  These  sample  points  capture  true  mean 
and  covariance  of  the  gaussian  random  variable.  In  UKF  approach  these  sample  points 
are  propagated  through  true  nonlinear  system  and  capture  posterior  mean  and  covariance 
accurately  up  to  second  order. 

2.3.1  Unscented  transformation 

It  is  difficult  to  transform  a  probability  density  function(pdf)  through  a  nonlinear  function. 
Unscented  transformation  is  a  method  of  calculating  statistics  of  random  variable  which 
undergoes  a  nonlinear  transformation.  Unscented  kalman  hlter(UKF)  philosophy  is  based  on 
unscented  transformation.  It  performs  a  nonlinear  transformation  on  a  single  point  rather 
than  a  pdf  as  well  as  it  provides  a  set  of  individual  points  in  state  space  whose  sample 
probability  density  function(pdf)  approximates  the  true  pdf  of  a  state  vector. 
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If  the  mean  and  covariance  of  a  state  vector  x  be  x  and  P  respectively.  Then  a  set 
of  deterministic  points  called  as  sigma  points  can  be  computed  whose  ensemble  mean  and 
covariance  are  equal  to  x  and  P.  The  dimension  of  the  state  vector  x  is  n.  Therefore,  we 
choose  2 n  sigma  points.  The  unscented  transformation  can  be  expressed  as  follows 


X W  =  X  +  X w 

*  =  1,2,... 

....2  n 

(2.32) 

x®  =  (VnP'j 

*  =  1,  2, .... 

...:n 

(2.33) 

x^n+i)  =  -  (^P)T 

*  =  1,2,.... 

....71 

(2.34) 

In  the  Eq. (2. 33)-Eq. (2. 34), \/nP  can  be  computed  by  Cholesky  decomposition.  In  camera 
sensor  model  each  camera  has  three  states  r,  9  and  (j)  therefore  the  value  of  n  is  3  in  the 
present  work  carried  out  for  state  estimation. 

2.3.2  Time  update  equations 

Pin-hole  camera  vision  sensor’s  n  state  nonlinear  system  can  be  expressed  as 


%k-\- 1  f  U'ki  tk)  W k 

(2.35) 

Vk  Vfc 

(2.36) 

wk  ~  (0,Qk) 

(2.37) 

vk  ~  (0,  Rk) 

(2.38) 

where  f(xk,Uk,tk)  is  nonlinear  transition  matrix  function  as  described  in  Eq.(2.15)  and 
h(xk,tk )  is  nonlinear  measurement  matrix  function  as  per  definition  but  in  present  work  it 
is  considered  linear  as  described  in  Eq.(2.28).  Q  and  R  are  process  noise  covariance  and 
measurement  noise  covariance  respectively. 

The  stereovision  camera  sensor  state’s  mean  and  covariance  can  be  initialized  as  per 
Eq.(2.39)-Eq.(2.40).  It  is  normally  assumed  that  it  is  known  priori  or  if  it  is  unknown 
then  there  is  precisely  a  bound  within  it  is  supposed  to  lie  based  on  the  physical  sensor 
measurement  precision  characteristics. 

=  E(x0) 

po  =  E[{%  -  ~  %t)T} 
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(2.39) 

(2.40) 


Time  update  equations  propagate  the  camera  sensor  state  estimate  based  on  sensor  non¬ 
linear  dynamics  and  the  averaging  of  all  sigma  points  state  estimates,  and  propagate  camera 
sensor’s  error  covariance  based  on  averaging  the  difference  between  sigma  point  estimates 
and  its  average.  Time  update  equations  Eq.(2.41)-Eq.(2.43)  for  state  propagation  can  be 
written  considering  previous  step  mean  and  covariance  as  per  Eq.(2.39)-Eq.(2.40)  as  best 
guess  value  for  computing  next  step  state  mean  and  error  covariance. 


xk- 1 

= 

/  / - \T 

*  =  1,2,... 

....2  n 

(2.41) 

=  (\A^), 

/  / - \T 

*  =  1,2,... 

. n 

(2.42) 

*  =  1,  2, .... 

. n 

(2.43) 

Using  the  camera  sensor  nonlinear  dynamics,  transform  the  sigma  points  obtained  in 
Eq.(2.41)-Eq. (2.43)  as 

x{k  =  /(4-nutifk)  (2-44) 


Transformed  sigma  points  obtained  through  nonlinear  dynamics  as  per  Eq.(2.44)  are 
again  averaged  and  hence,  the  prior  state  estimate  at  time  k  is  obtained  as  follows 

k  =  (2-45) 

Similarly,  prior  error  covariance  taking  process  noise  into  account  can  be  propagated  as 
follows. 


Pi 


1 

2  n 


%k  )T  +  Qk-l 


(2.46) 


2.3.3  Measurement  update  equations 

Compute  the  sigma  points  again  with  current  best  guess,  which  is  the  prior  mean  and 
covariance  computed  in  the  previous  steps  as  per  Eq.(2.44)-Eq.(2.46),  the  following  step  is 
carried  out  to  enhance  the  performance  of  Unscented  kalman  filter. 


aW 

xk 

=  xk  +x(l> 

(  / - \T 

*  =  1,2,... 

....2  n 

(2.47) 

X® 

-  (^), 

/  r - U 

*  =  1,2,... 

. n 

(2.48) 

j.(n+i) 

*  =  1,2,... 

. n 

(2.49) 
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Camera  sensor  measurement  model  is  used  to  transform  the  sigma  points  in  previous  step 
as  per  Eq.(2.47),  into  predicted  measurements.  The  measurement  equation  is  as  follows. 

y(k  =  K^k\tk)  (2.50) 

Taking  average  of  all  predicted  measurements  obtained  as  per  Eq.(2.50),  which  are  cor¬ 
responding  to  the  sigma  points,  the  predicted  measurement  output  can  be  computed  as 
follows, 

fe  =  <2-51) 

The  covariance  of  predicted  measurement  can  be  computed  with  from  Eq.(2.50)  and 
yk  from  Eq.(2.51)  as  follows, 

pv  =  ^  ^2(Vk]  ~  yk){y{k  -  i)k)T  +  Rk  (2.52) 

which  is  based  on  difference  between  predicted  measurement  and  its  average. 

Similarly,  cross  covariance  between  the  difference  between  from  Eq.(2.44)  and  prior 
estimate  of  camera  state  x from  Eq.(2.45)  and  the  difference  between  from  Eq.(2.50) 
and  predicted  measurement  output  ijk  from  Eq.(2.51)can  be  computed  as 

pxy  =  -£k)(yk]  -yk)T  (2.53) 

The  measurement  update  equation  involving  kalman  gain  computation  is  provide  in 
Eq.(2.54),  the  required  inputs  Py  and  Pxy  for  kalman  gain  computation  are  obtained  from 
previous  steps.  The  measurement  update  equation  for  posterior  camera  state  or  the  cor¬ 
rected  state  estimate  is  provided  in  Eq.(2.55),  the  required  inputs  are  obtained  from  previous 
steps.  The  measurement  update  equation  for  posterior  covariance  computation  is  provided 
in  Eq.(2.56),  the  required  inputs  are  obtained  from  previous  steps.  The  posterior  state  esti¬ 
mate  and  posterior  covariance  obtained  in  the  following  equations  are  again  utilized  as  the 
best  guess  value  for  next  step  iteration. 


Kk 

—  P  P~l 

1  xy1  y 

(2.54) 

K 

=  x^  +  Kk(yk-yk) 

(2.55) 

p+ 

rk 

=  P^-KkPyKl 

(2.56) 
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2.4  Velocity  Estimation 


Reactive  collision  avoidance  for  moving  obstacles  involves  estimation  of  obstacle  position 
as  well  as  obstacle  velocity  [7].  The  projection  of  the  moving  obstacle  on  two  camera  image 
planes  in  the  geometrical  configuration  shown  in  Fig.  2.1  provides  the  instantaneous  posi¬ 
tion  of  the  obstacle.  However,  the  iterative  projections  of  the  moving  obstacle  acquired  by 
the  camera  sensors  provide  the  iterative  positions  of  the  obstacle  and  using  the  difference 
equation  for  iterative  positions  and  corresponding  time  instants  the  velocity  of  the  obstacle 
can  be  computed.  The  computed  velocity  serves  as  a  reference  or  measured  velocity. 

The  camera  sensors  are  noisy  and  hence,  the  position  and  velocity  computed  by  the  trian- 
glization  method  is  inaccurate.  Therefore,  EKF  or  UKF  techniques  are  used  for  estimating 
the  accurate  position  and  velocity  of  the  obstacle  from  the  inaccurate  data.  The  camera 
sensor  dynamics  is  first  order  differential  equation.  The  sensor  dynamics  is  propagated  and 
the  updated  position  information  is  obtained.  Similarly,  differentiate  the  sensor  dynamics 
to  obtain  second  order  state  derivative  and  propagate  the  second  order  sensor  dynamics 
and  obtain  the  velocity  information.  It  is  assumed  that  the  function  is  differentiable  at  all 
instants  of  interest.  Whenever,  obstacle  is  ahead  of  camera  and  visible  through  both  the 
sensors. 

The  guidance  command  uses  the  estimated  obstacle  position  as  well  as  estimated  obstacle 
velocity  to  compute  the  aiming  point.  The  prediction  logic  decides  in  advance  that  what 
conld  be  the  point  of  intersection  or  what  could  be  minimum  distance  between  UAV  and 
the  moving  obstacle.  The  minimum  distance  is  computed  and  it  is  compared  with  the  safety 
boundary  limit  of  the  obstacle  if  the  minimum  distance  is  less  than  the  safe  distance  then  the 
guidance  command  is  applied  to  divert  the  velocity  vector  of  the  UAV  so  that  the  upcoming 
collision  can  be  averted.  The  Fig.2.2  shows  the  miss  distance  between  UAV  and  the  moving 
obstacle. 

2.4.1  Relative  Velocity  Estimation 

Since  the  left  and  right  cameras  mounted  on  UAV  receive  the  projection  of  the  obstacle 
on  their  image  planes  if  the  obstacle  lies  in  the  field  of  view  (FoV)  of  the  cameras.  The 
geometrical  configuration  discussed  in  the  previous  section.  The  initial  guess  for  position 
and  velocity  of  the  obstacle  can  be  obtained  from  the  sequential  data  frames  obtained  from 
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stereo  camera  sensors.  Since  UAV’s  own  position  and  velocity  is  known.  The  vision  sensors 
provide  the  relative  position  and  relative  velocity  estimate  of  the  obstacle.  The  absolute 
position  and  velocity  of  the  obstacle  can  be  estimated. 

After  initialization,  the  position  as  well  as  velocity  of  the  obstacle  is  propagated  using- 
sensor  dynamics  as  well  as  the  dynamics  obtained  by  taking  the  derivative  of  the  sensor 
dynamics  respectively.  The  assumption  made  here  is  that  obstacle  is  moving  with  constant 
velocity  for  simplicity.  The  relative  position  between  UAV  and  the  obstacle  can  be  written 
as 


A  rpl 


1  T 


%rel  Vrel  %rel 


(2.57) 


The  above  equation  can  be  represented  for  UAV  in  polar  coordinates  considering  the 
equation  in  inertial  frame.  Where  rrei  ,  6rei  and  <prei  are  the  relative  distance  and  angles 
between  UAV  and  obstacle.  The  expression  for  r rei  is  as  follows. 


lYei 


Trel  COS  ((firei)  COS  (9rei)  rrei  COS  (0re;)  sin  {Orel) 


~r rel  Sill ((firel) 


T 


(2.58) 


The  Eq.(2.57)  can  be  written  for  UAV  in  polar  coordinates  considering  the  equation  in 
velocity  frame.  Where  rrei  ,  Xrel  and  7 rei  are  the  relative  distance  and  angles  between  UAV 
Velocity  and  axes  system.  The  velocity  frame  representation  is  as  follows. 


r rel 


r rel  COS (7 rei)  COS (Xrel)  del  COS (7 rel)  Sm(Xre*) 


~rrel  Sill(7rcZ) 


T 


(2.59) 


Taking  first  derivative  of  the  Eq.(2.59)  the  relative  velocity  between  UAV  and  the  obstacle 
can  be  written  as  [1] 


1 

"5 

COS  7 rei  COS  Xrel  ~T  COS  Xrel  sin  Xrel  ~r  sin  Xrel  COS  Xrel 

f  rel 

Vyrel 

= 

COS  Xrel  Sin  Xrel  r  COS  Xrel  COS  Xrel  ~T  sill  Xrel  SU1  Xrel 

Xrel 

1 

"5 

-sill  7 ret  0  -r  cos  Xrel 

Xrel 

The  absolute  velocity  of  the  obstacle  can  be  computed  by  adding  the  relative  velocity  with 
UAV  velocity.  UAV’s  velocity  and  acceleration  information  is  known  through  it’s  dynamics 
and  guidance  commands.  UAV’s  position  as  well  as  velocity  information  is  also  known 
through  it’s  own  predefined  global  path  planning  or  through  the  onboard  computation  as 
in  local  path  planning.  The  absolute  velocity  of  the  obstacle  can  be  computed  through  the 
following  equation 
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Vabs  —  Vuav  +  Vrel  (2-61) 

The  steps  followed  in  velocity  estimation  is  first  the  camera  sensor  dynamics  is  providing 
the  expected  sensor’s  states  r,  9,  (j)  then  with  UAV’s  initial  Velocity  vector,  initial  7  and 
initial  y  ,  it  is  possible  to  propagate  the  UAV  system  dynamics  which  is  considered  as  Point 
mass  model  and  obtain  V,  y,  7  as  well  as  x,  y,  z  after  the  state  propagation  the  UAV  location 
is  updated  and  Sensor  measures  the  the  new  states  and  again  sensor  dynamics  propagates 
and  the  next  sensor  states  are  obtained. 

Using  the  obstacle  projection  on  pin-hole  camera  sensor  which  are  obtained  from  image 
processor  at  each  iteration,  the  relative  position  of  the  obstacle  can  be  computed.  Using  the 
successive  position  at  each  time  step,  the  difference  equation  is  used  to  compute  the  velocity 
of  the  obstacle.  This  velocity  can  be  inferred  or  termed  as  the  measured  velocity. 

The  estimated  velocity  can  be  computed  using  the  following  steps. 

(1) .  Using  the  sensor  states  and  their  derivatives  compute  the  relative  velocity  between 
obstacle  and  UAV  as  per  Eq.(2.60) 

(2) .  Take  the  second  derivative  of  sensor  dynamics  and  compute  the  second  order  derivative 
of  the  sensor  states. 
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—  cos  9  sin  <fi(j)Vxrei  —  sin  9  cos  (j)9Vxrei  +  cos  9  cos  4>Vxrei 

—  sin  9  sin  (jxj)Vyrei  +  cos  9  cos  <p9Vyrei  +  sin  9  cos  (j)Vyrei 

+vzrei  COS  (/*/>  +  Vzrd  sin  (j) 

r  cos  (j)( —  cos  06Vxrei—  sin  9Vxrei~Vyrei  sin  90+cos  0Vyrei) 
r 2  cos2  (p 

—  (—  sin  0Vxrei+c os  0Vyrei)(—r  sin  cos  <j)r) 

r 2  cos2  cf) 

r(—Vxrei( cos  9  cos  </></>— sin  cj>  sin  99)— cos  9  sin  4>VXrel)+{Vxrel  cos  9  sin  4>)r 

r(—Vyrei  (sin  9  cos  </></>+sin  cf)  cos  99)—  sin  9  sin  0V^rej)+(V^rez  sin  9  sin  cj))r 
“1  ^2 

i  r(-Vzrei  sin  (f)(j)-\-cos  4>Vzrei)  Vzrei  cos  (f>r 

i  r2 


(2.62) 


Sensor  state  second  order  derivatives  as  obtained  in  Eq.(2.62)  are  function  of  sensor  states 
and  their  first  order  derivatives  as  described  in  Eq.(2.15),  relative  velocity  as  per  Eq.(2.60) 
as  well  as  derivative  of  relative  velocity  as  described  in  Eq.(2.66)-  Eq.(2.68). 

Since,  Obstacle  velocity  is  assumed  to  be  constant,  the  derivative  of  the  obstacle  velocity 
is  zero.  The  UAV  velocity  is  given  by  the  Eq.(A.l-A.3)  then  by  computing  the  derivative  of 
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UAV  velocity  the  derivative  of  relative  velocity  can  be  computed. 


Vrei  =  V0-VU 

Taking  first  derivative  of  Eq.(2.63) 

Vrel  =  V0~VU 

Since  obstacle  velocity  is  constant  Eq.(2.64)  can  be  written  as 

breZ  Vu 


(2.63) 


(2.64) 


(2.65) 


The  equations  for  UAV  velocity  Vu  in  inertial  frame  is  provided  in  the  Eq.(A.l-A.3), 
therefore  for  Vu  the  Eq.(2.65)  can  be  written  as 


Vx rei  =  VT  cos(x)  sin(7)7  +  cos(7)  sin(x)x  -  cos(x)  sin(7)UT  (2.66) 

Vyrei  =  UTsin(x)sin(7)7-cos(7)cos(x)x-sin(x)cos(7)UT  (2.67) 

Vzrei  =  -UTcos(7)7  -  sin(7)UT  (2.68) 


(3).  Propagate  the  sensor  state  derivative  and  using  the  state  derivative  compute  the 
propagated  the  relative  velocity.  The  obstacle  velocity  is  computed  from  the  data  received 
from  the  camera  sensors  using  difference  equation  as  follows 


17/  rn 


Xobs(t  +  dt)  -  Xobs(t) 
dt 


(2.69) 


The  obstacle  Velocity  obtained  through  Eq.(2.69)  is  assumed  as  obstacle  velocity  through 
camera  sensor  measurement  as  obstacle  positions  are  computed  by  triangulization  method  at 
each  instant.  However,  the  obstacle  velocity  obtained  by  Eq.(2.61),  which  involves  relative 
velocity  computation  and  is  achieved  through  sensor  dynamics  propagation.  The  EKF  or 
UKF  filter  is  used  to  compute  the  estimated  absolute  velocity  of  the  obstacle  as  follows. 


Vestik  +  1)  —  Vest(k)  +  Kv(Vabs  —  Vcam )  (2.70) 

where, Kv  is  kalman  filter  gain. 

Since,  the  UAV  velocity  is  known  and  relative  velocity  between  UAV  and  obstacle  is 
known  the  absolute  velocity  of  obstacle  can  be  computed. 
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2.4.2  Reactive  collision  avoidance  with  moving  obstacles 

UAV  and  the  obstacle’s  initial  position  is  Xu(0)  and  Xo(0)  and  their  respective  velocity 
is  Vu  and  VQ.  UAV  is  moving  at  constant  velocity  as  no  guidance  is  working  and  obstacle 
is  assumed  to  be  moving  with  constant  velocity  as  per  Fig.  2.2.  The  predicted  positions 
Xu(t)  and  Xa(t)  of  the  UAV  and  the  obstacle  with  current  velocities  after  time  t  is  given  by 
Eq.(2.71)-Eq.(2.72).The  prediction  logic  tells  the  future  positions  of  UAV  and  obstacle  and 
also  the  minimum  separation  between  UAV  and  obstacle.  Therefore,  it  is  possible  for  UAV 
to  take  corrective  action  in  due  course  of  time  and  avoid  any  future  collision  by  applying  the 
guidance  command  and  changing  it’s  velocity  profile. 


Figure  2.2:  Non-cooperative  conflict  resolution 


Xu(t)  =  Xu(0)  +  t*Vu  (2.71) 

Xo{t)  =  Xo(0  )  +  t*V0  (2.72) 

Relative  distance  predicted  between  UAV  and  obstacle  can  be  computed  at  any  time  t 
as  per  Eq.(2.73),  which  is  a  vector  equation.  The  Euclidian  norm  of  Rrei(t)  provides  the 
predicted  distance  d(t)  in  scalar  form  and  discussed  later  in  the  section. 

Rrel(t)  =  Xu(t)-X0(t)  (2.73) 

Subtracting  Eq.(2.72)  from  Eq.(2.71)  and  substituting  with  Eq.(2.75)  and  Eq.(2.73),  the 
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following  equation  can  be  obtained. 


Rrelif)  —  -Rre/(0)  +  t(Vu  ~  V0)  (2.74) 

Where, Eq.  (2.75)  describes  the  initial  separation  between  UAV  and  obstacle. 

Rreli  0)  =  AA(0)-Xo(0)  (2.75) 

The  objective  here  is  to  compute  the  minimum  separation  between  the  UAV  and  the 
obstacle  and  the  corresponding  predicted  time.  The  predicted  point  when  the  UAV  just 
intersect  each  other  or  at  a  minimum  distance  to  each  other  the  minimum  Distance  d(t) 
should  be  Minimum  of  ||i?.rez(f)||.  In  order  to  compute  the  minimum  distance 

D(t)  =  d(t)2  =  \\Rrel(t)\\2  (2.76) 

=  Rrel(t)TRrel(t )  (2.77) 

=  (yRrei  (0)  +  tVu  —  tVo)r(Rrel(0)  +  tVu  —  tVo)  (2.78) 

D(t)  =  (. RrelmT(Rreim  +  2(RreimT(Vu-V0)t  +  (Vu-V0)T(Vu-V0)t 2  (2.79) 

Differentiating  the  Eq.(2.79)  for  D(t)  with  respect  to  time  t  and  solving  for  minimum 
distance  we  obtain  as  follows. 


=  2(Vu-Vo)T(Vu-Vo)t  +  2(Rrel(0))T(Vu-Vo) 
dt 

For  D(t)  to  be  minimum, 

dm  = 

dt 


(2.80) 


(2.81) 


Solving  the  Eq.(2.81)  for  t,  which  is  called  as  ’’time  of  closest  approach”  tc  when  the 
UAV  and  the  obstacle  are  closer  to  each  other  as  per  Eq.(2.82).  tc  is  the  available  time  for 
UAV  to  maneuver  and  avoid  the  predicted  collision  depending  on  the  predicted  minimum 
separation. 


Rrel(0)Ti,Vu  -  V0) 

IIK-KII2 


(2.82) 


The  distance  between  UAV  and  obstacle  at  the  time  of  closest  approach  is  called  as  miss 
distance  or  zero  effort  miss  distance  the  ZEM  can  be  computed  as 
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Substituting  the  value  of  tc  from  Eq.(2.82)  in  Eq.(2.83)  the  predicted  miss  distance  rm  is 
computed  as 


rm  =  Xu(tc )  -  XQ(tc )  (2.83) 

Where  Xu(tc )  and  X0{tc)  are  the  predicted  position  of  UAV  and  obstacle  at  time  tc  for 
predicted  reactive  collision  to  occur  the  folowing  conditions  must  be  satisfied  (i)  If  tc  >  0  the 
UAV  and  obstacle  are  coming  closer  to  each  other  therfore  the  probable  conflict  is  possible. 
If  tc  <  0,  then  UAV  and  obstacle  are  moving  away  from  each  other,  (ii)  If  ||rm||  <  rsafe 
then  predicted  position  of  UAV  is  within  the  safety  sphere  of  obstacle  and  it  can  be  inferred 
that  collision  has  occured.  Where,  rsafe  is  the  safety  radius  of  the  sphere.  If  both  conditions 
are  satisfied  simultaneously  then  the  collision  is  said  to  be  occurred  at  the  time  of  closest 
approach  tc.  Therefore,  the  effective  guidance  commands  must  be  computed  to  avert  the 
collision.  In  non-cooperative  scenario  where  obstacle  is  autonomously  moving  the  UAV  has 
to  maneuver  to  avoid  collision.  The  UAV  has  to  deflect  from  its  predicted  position  by  residual 
distance  rres  which  is  given  in  Eq.(2.84)  so  that  the  rsafe  separation  can  be  insured. 


T res  rsafe  ||Un||  (2.84) 

The  Eq.(2.84)  can  be  incorporated  by  changing  the  velocity  vector  of  the  UAV  and  the 
desired  velocity  of  UAV  VUdes  is  provided  in  Eq.(2.85).  The  UAV  velocity  vector  need  to 
align  along  the  desired  velocity  so  that  the  desired  aiming  point  can  be  achieved. 


■rr  _  Wc  T  T vm 

udes  1 1  T/  f  rr>  I 

||  v  U^C  ~  1  vm  | 


(2.85) 


where,  rvm  is  defined  in  the  Eq.(2.86),  it  is  a  distance  vector  with  magnitude  rres  and 
direction  rm. 


T'vm 


^  rps'T’  r, 


(2.86) 


Therefore,  the  differential  geometric  guidance(DGG)  can  be  invoked  to  demand  the  de¬ 
sired  velocity  Eq.(2.85)  and  correct  the  predicted  position  of  the  UAV  to  avoid  collision  with 
obstacle. 
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Chapter  3 

Collision  Avoidance  Philosophy  and 
Guidance  Design 


In  the  present  work,  it  is  assumed  that  global  path  planning  has  already  been  done  off  line 
(or  possibly  loaded  to  the  onboard  processor  through  telemetry),  where  a  path  towards  the 
goal  point  has  already  been  found  accounting  for  the  known  obstacles  in  the  environment. 
The  UAV  is  equipped  with  an  onboard  camera  with  necessary  image  processing  algorithms  to 
sense  an  unforseen  obstacle,  along  with  its  location,  in  its  close  vicinity.  The  main  focus  then 
is  to  predict  a  possible  collision  with  the  obstacle,  and  if  so,  to  steer  it  away  with  appropriate 
generation  of  guidance  commands.  Details  of  this  guidance  algorithm  are  discussed  in  this 
section. 
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3.1  Aiming  Point  Computation 


Once  the  obstacle  information  is  available  through  onboard  sensors,  the  task  is  to  predict 
possible  collision,  and  if  so,  to  steer  it  away  by  appropriately  computing  of  aiming  point  and 
generation  of  required  guidance  commands.  The  philosophy  behind  computing  the  aiming 
point  is  based  on  collision  cone  approach  has  physical  relevance  with  torch  light  experiment, 
where  if  the  torch  light  is  glown  to  the  sphere,  the  locus  of  extreme  points  of  the  glow 
area  will  form  a  circle  on  the  sphere  and  the  rays  passing  through  the  extreme  points  are 
tangent  to  the  sphere.  The  same  analogy  is  replicated  in  3D  representation  of  collision  cone 
containing  UAV’s  CG  at  coordinate  frame  origin,  point  obstacle  with  safety  sphere,  plane 
and  a  circle  as  shown  in  Fig.  3.1.  Note  that  if  one  draws  tangents  to  this  sphere  from  the 
CG  of  the  UAV,  it  results  in  a  ’cone’  (hence  it  is  called  as  the  ’collision  cone’  [41]),  and  the 
circle  formed  by  locus  of  tangents  contact  point  on  sphere  is  named  as  collision  circle  and 
represented  as  Cv 


Figure  3.1:  Collision  Cone  Representation 

The  coordinate  frame  assumed  in  Fig.  3.1  is  parallel  to  the  inertial  frame  and  located 
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on  the  centre  of  gravity  (CG)  of  the  UAV.  Let  the  UAV  fly  along  the  direction  of  the  Vr- 
It  is  also  assumed  that  the  angle  of  attack  and  sideslip  angles  are  small  so  that  vehicle 
body  X-axis  (along  which  the  onboard  sensor  is  installed)  can  detect  obstacles  along  the  Vt 
direction.  Once  an  obstacle  is  detected  by  onboard  sensors,  collision  avoidance  algorithm  is 
invoked  and  a  new  aiming  point  is  computed.  As  per  collision  cone  approach  the  computed 
aiming  point  is  tangent  to  the  sphere,  so  it  should  he  on  this  collision  circle.  The  centre  and 
radius  of  the  collision  circle  is  determined  by  using  the  information  of  obstacle  sphere  radius 
and  distance  between  UAV  and  centre  of  the  obstacle.  From  Fig.  3.1,  let  us  consider  the 
A XvWX0b-  Where,  Xy  is  the  UAV  position,  IF  is  a  point  on  the  collision  circle  and  Xob 
is  the  obstacle  position.  Then  by  applying  Pythagoras  theorem  on  A XvWXqb 


M2HNI2  +  u2  (3.i) 

M|2  =  W2-r2  (3.2) 

||Xr||=mi  +  m2  (3.3) 

where  rs  is  the  radius  of  the  safety  sphere,  rt  is  the  tangent  vector  joining  UXV  position 
Xy  and  W,  the  line-of-sight  (LOS)  Xr,  by  definition,  is  the  line  joining  the  CG  of  the  UXV  to 
the  centre  of  the  obstacle.  Xc  is  the  centre  of  the  collision  circle  and  the  Euclidean  distance 
between  this  centre  to  Xy  is  rrii  and  to  XOB  is  m2  respectively.  Here  rn i  and  m2  are  also 
the  components  of  the  line  segment  ||Xr||.  Applying  Pythagoras  theorem  on  XXyWXc- 

Hull 2  =  m\  +  rl 

rc  =  \\rt\\2  —  mi  (3-4) 

Where  rc  is  the  radius  of  the  collision  circle,  from  XXyW Xqb 


r 


2 

S 


2  ,  2 
m2  +  rc 


(3.5) 


m2  =  rs  ~  rc 


=  u 


u 


1 2  ,  2 
|  +  m1 


(3.6) 
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To  find  the  value  of  mi  and  m2,  Squaring  Eq.(3.3)  on  both  sides  and  substitute  Eq.(3.6), 
yields 


||A"r||2  =  (mi  +  m2)2  =  rn\  +  m2  +  2  mlm2 


(3.7) 


mi 


m2 


\\Xr\\2  ~  r*  +  \\rtf 

2|Pr|| 


Dr 


Dr 


The  m2  value  can  be  find  out  by  substituting  the  value  of  mi  in  Eq.  (3.3) 


(3.8) 

(3.9) 


rl  = 


2  2 
—  rn\ 


rr  = 


Dr 


The  centre  of  the  collision  circle  is  given  as 


(3.10) 


Xc  —  Xv  +  —  (Xqb  ~  Xv)  (3-11) 

Let  us  consider  the  plane  77,  which  contains  the  collision  circle  Cv .  To  find  the  intersection 
point  between  the  plane  rj  and  velocity  vector  Vt  projected  in  time  t  from  the  UAV’s  current 
position.  Dne  to  geometrical  symmetry,  the  vector  joining  collision  circle  centre  (xc,  yc,  zc) 
and  obstacle  centre  (x0b,y0b,Zob)  is  always  perpendicular  to  plane  rj  .  Therefore,  the  normal 
vector  n  to  the  plane  can  be  described  as 


h  =  (a,  b ,  c)  =  ( xob  -  xc,  yob  -  yc,  zob  -  zc)  (3.12) 

The  general  plane  equation  passing  through  a  point  (xc,  yc,  zc)  is 

a(x  -  xc)  +  b(y  -  yc)  +  c(z  -  zc)  =  0  (3.13) 

The  position  vector  of  UAV  is  expressed  in  terms  of  instantaneous  velocity  vector  pro¬ 
jected  in  time  t  is  expressed  as 

Xt  =  Xv  +  tV  (3.14) 
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Substitute  above  equation  into  plane  equation  and  solve  for  desired  time  t*,  which  is 
termed  as  time  to  reach  the  plane 


a(xv  +  tu  —  xc)  +  b(yv  +  tv  —  yc)  +  c(zv  +  tw  —  zc)  =  0 


(3.15) 


t*  = 


ft  •  (Xy  -  XC) 
n  ■  V 


The  intersection  point  is  given  as 


Xf  =  xv  +  t*v 


(3.16) 


(3.17) 


Once  intersection  point  is  computed,  the  following  conflict  conditions  are  evaluated 

1.  \\Xt*  —  Xq 1 1  <  Radius  of  the  circle 

2.  t*  >  0 

If  the  above  conditions  are  satisfied  then  the  obstacle  under  consideration  is  said  to  be 
critical,  then  the  new  aiming  point  is  computed  as  follows 

Ad-  -  Xc 


x“”-Xc+rlw-xcl 


(3.18) 


Note  that,  when  no  obstacle  is  critical,  the  goal  point  becomes  the  aiming  point,  i.e. 
A  ap  X goal- 

The  collision  avoidance  problem  therefore  can  be  interpreted  as  a  sequential  target  in¬ 
terception  problem  or  way  point  guidance  problem  with  angle  constraints  at  intermediate 
targets  or  way  points.  However,  the  basic  difference  is  the  online  fixing  of  these  intermedi¬ 
ate  points  and  then  quickly  realigning  the  velocity  vector  to  visit  them  this  is  done  by  the 
computation  of  required  guidance  commands  as  follows.  Once  the  aiming  point  is  computed, 
the  task  then  is  to  follow  the  philosophy  of  ’aiming  point  guidance’  of  missile  guidance  lit¬ 
erature  [18]  (which  has  a  close  resemblance  to  pursuit  guidance  [33])  and  align  the  vector 
towards  this  aiming  point.  For  carrying  out  the  necessary  algebra  (see  Fig.  2),  the  obstacle 
coordinates  are  given  by  X0b  =  [xQb  y0b  z0b]  and  the  relative  aiming  point  vector  is  given  by 
Xvap  =  [ xvap  yvap  zvap\.  Once  these  co-ordinates  are  known  (with  respect  to  the  coordinate 
frame  at  CG  that  is  parallel  to  the  intertial  frame),  the  desired  flight  path  angle  yc  and 
course  angle  \c  can  be  calculated  as: 
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7 


(3.19) 


=  tan 


=  tan 


(3.20) 


Note  that  even  though  it  may  sound  logical  to  slow  down  (i.e.  to  reduce  Vp)  until  the 
collision  is  avoided,  in  this  work  it  is  proposed  to  hold  Vr  at  its  initial  value  at  the  start 
of  guidance  (i.e.  Vf  =  Vt{0)  ).  This  is  because  the  time  availability  is  small  and  very  less 
correction  can  be  done  for  it  within  the  available  small  time-to-go  (usually  thrust  can  be 
varied  only  slowly).  The  angles  7  and  y  as  well  as  Vf  are  the  necessary  guidance  commands 
in  the  formulation  using  the  kinematic  model.  In  Fig.  2,  Vr(x,y)  is  the  projection  of  the 
velocity  vector  and  Xvap(x,y)  is  the  projection  of  the  revised  LOS  in  the  horizontal  plane 
respectively.  For  the  formulation  using  the  point  mass  model,  however,  one  needs  to  carry 
out  further  algebra  to  generate  the  physically  meaningful  guidance  commands,  i.e.  the  angle 
of  attack,  bank  angle  and  thrust  commands.  Note  that  the  alignment  of  velocity  vector 
needs  to  be  quickly  carried  out  within  a  fraction  of  the  available  time-to-go,  thereby  making 
it  possible  to  avoid  pop-up  obstacles.  For  this  to  happen,  the  information  regarding  time- 
to-go  to  the  aiming  point  first  need  to  be  known,  which  is  obtained  in  following  manner. 
Note  that  Vr  needs  to  be  aligned  to  the  vector,  which  is  located  at  the  CG  of  the  vehicle 
and  points  towards  the  aiming  point.  This  vector  can  be  interpreted  as  ’Revised  LOS’,  the 
magnitude  of  which  is  given  by 


R  =  \\X, 


vap  ||  2  —  \/ Xvap 


—  .  /^2 


+  ylav  +  Zv 


(3.21) 


Assuming  that  Vr  remains  fairly  constant  (which  is  true,  as  our  formulation  attempts  to 
assure  it),  the  time-to-go  to  reach  the  aiming  point  can  be  computed  as  follows. 


tgo 


V  •  VT) 

\\Vr\\2  ‘ 


R 

Vp 


(3.22) 


Next,  the  settling  times  of  the  imposed  error  dynamics  are  selected  as  a  fraction  of  this 
time-to-go  and  gain  values  are  selected  accordingly. 
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3.2  Nonlinear  Differential  Geometric  Guidance 


The  present  work  assumes  a  goal  point  in  the  environment  with  unforseen  obstacles  whose 
instantaneous  locations  are  known  through  onboard  sensors.  Aiming  point  is  calculated  with 
the  collision  cone  approach  [41].  The  aiming  point  is  pursued  through  nonlinear  geometric 
guidance  law.  In  the  present  work,  the  velocity  magnitude  of  UAV  is  kept  constant  and  only 
directions  are  manipulated  to  execute  the  guidance  law.  The  objective  of  the  guidance  law 
is  to  steer  the  UAV  to  the  aiming  point  through  correction  of  velocity  direction  both  in  the 
vertical  and  the  horizontal  plane.  This  implies  the  desired  values  for  the  orientation  of  the 
velocity  vector  in  both  the  planes  respectively  will  become 

7C  =  K,  Xc  =  Aa 

as  evaluated  from  Eq.  (3.19)  and  Eq.  (3.20).  To  execute  the  guidance  law  with  the  point 
mass  model  as  described  by  Eqs.  (A. 7)  -  (A. 12),  the  necessary  forces  are  required  to  stir  the 
vehicle  in  the  desired  direction.  Forces  required  by  the  UAV  are  observed  in  form  of  three 
control  variables-desired  angle  of  attack  ad  ,the  desired  bank  angle  fid  and  the  desired  thrust 

Td. 

3.3  Controller  Design 

The  control  commands  fid  and  ad  are  required  to  achieve  the  commanded  horizontal  and 
vertical  flight  path  angles  whereas  Td  is  required  to  track  the  desired  velocity  of  the  vehicle. 
Desired  bank  angle  /j,d  is  generated  in  the  closed  form  as  follows: 

mVr  cos  yx  =  (T  sin  a  +  L)  sin  /i  (3.23) 

mV t7  +  Trig  cos  7  —  (T  sin  a  +  L)  cos  /i  (3.24) 

Desired  bank  angle  nd  is  generated  by  using  the  philosophy  of  NDI  [35].  Enforcing  the  first 
order  error  dynamics  on  \  and  7  we  get 

X  =  -kx(.X~Xc)  (3.25) 

7  =  -M7~7c)  (3-26) 

Since  the  bank  angle  is  appearing  in  Eq.  (3.23)  and  in  Eq.  (3.24)  in  affine  form,  so  both  the 
equations  are  used  to  find  the  desired  value  of  the  bank  angle.  By  dividing  Eq.  (3.23)  by 
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Eq.  (3.24)we  get, 


rnVr  cos  yy 
rnVrj  +  mg  cos  7 


tanna  — 


(3.27) 


arctan 


mVTcos'y(-kx(x  ~  Xc )) 

rriVr(—k1(ry  —  7C))  +  mg  cos  7 


(3.28) 


The  desired  thrust  is  required  by  the  vehicle  to  maintain  the  velocity  to  its  desired  value 
Vt° ■  Vt°  is  the  initial  velocity  of  the  vehicle.  It  is  assumed  that  in  the  short  duration  of 
obstacle  avoidance  the  velocity  of  UAV  remains  constant.  The  first  order  error  dynamics  for 
the  velocity  can  be  written  as 


Vp  —  — ky  (Vr  —  Vrc) 


(3.29) 


It  can  be  seen  from  Eqs.  (A. 10),  (A.ll)and  (A. 12)  that  both  the  thrust  and  angle  of  attack 
(a)  are  dependent  on  each  other  and  hence  closed  form  solution  for  both  control  variables 
cannot  be  deduced  by  algebraic  manipulation.  Vt  dynamics  in  Eq.  (A.  12)  can  be  rewritten 
as 

T  cos  a  —  D  =  mVr  +  mg  sin  7  (3.30) 

By  substituting  Eq.  (3.29)  in  Eq.  (3.30)  we  get, 

T  cos  a  —  D  =  m(—kvVT  —  VTC)  +  mg  sin  7  (3.31) 

Similarly,  by  squaring  and  adding  equation  Eqs.  (3.23)  and  (3.24)  we  get, 

(T  sin  a  +  L )2  =  (mVT  cosyy)2  +  (■ mVr'y  +  mg  cosy)2  (3.32) 

(Tsina  +  L)  =  a J ( mVr  cos  77) 2  +  (mVr'y  +  mg  cos  7) 2  (3.33) 

By  substituting  Eq.  (3.25)  and  Eq.  (3.26)  in  Eq.  (3.33)  we  get, 

(T  sin  a  +  L)  =  \J  (mVr  cos7(  —  kx(x  —  Xc)))2  +  JmVr{—k^{ 7  —  7C))  +  mg  cosy)2  (3.34) 

The  two  Eqs.  (3.31)  and  (3.34)  are  nonlinear  and  have  two  unknowns  and  ay.  ay  and 
are  generated  by  solving  numerically,  simultaneously  Eqs.  (3.31)  and  (3.34)  through  Newton 
Raphson  method  [40].  The  initial  guess  values  of  the  control  variables  for  Newton  Raphson 
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method  are  taken  as  trim  values  a  =  3.314°  and  T  =  5.561V.  Learning  rate  parameter  of  0.6 
is  introduced  for  both  the  variables  for  smooth  convergence.  The  convergence  criteria  is  set 
as  the  bounded  relative  error  in  Td  and  ad  or  the  maximum  number  of  iteration  assigned  for 
convergence.  Bounded  relative  error  can  be  stated  as 

—  <  Toh  ^  <  Tol2  (3.35) 

a  1 

where  Toll  and  T0I2  are  respective  tolerance  limits  of  the  relative  error  in  Td  and  ad-  Toll 
and  Tol2  can  be  taken  differently  but  for  present  study  Toll  —  Tol2  =  0.1%.  The  maximum 
number  of  iterations  assigned  for  convergence  is  26.  The  time  for  convergence  is  around  lms 
which  is  lesser  than  one  time  update  cycle  (20ms). 

3.3.1  Autopilot  Compensation 

The  control  variables  //*,  a*,  T*  are  passed  through  the  autopilot  to  account  for  internal 
dynamics.  The  autopilot  introduces  a  first  order  delay  in  response  which  is  compensated  by 
designing  an  autopilot  controller  in  all  the  three  control  channels.  It  is  assumed  that  the 
autopilot  states  (control  variables  fid,  ad,  Td)  are  available  for  the  feedback.  The  controller 
is  designed  based  on  the  error  of  the  actual  states  of  the  autopilot  fid,  ced,  Td  and  the  desired 
state  of  the  autopilot  fi* ,  a*,  T*  respectively.  The  error  in  the  channel  of  bank  angle  is 
given  by  e  =  fid  —  fi* .  Enforcing  the  Erst  order  error  dynamics  on  the  bank  angle  we  get 


(fld  -  fi*)  +  k^dc (fid  ~  n*)  =  0 

(3.36) 

fid  =  fi*  —  k/j,dc  (fid  —  fi  ) 

(3.37) 

Substituting  the  autopilot  dynamics  from  the  Eq.  (A.  15)  in  Eq. 

(3.37)  and  rearranging  we 

get 

Tdc  T  [kfj,dTd  +  T*  (Td  aO] 

(3.38) 

desired  rate  of  change  of  control  fi*  =  0  ,  therefore, 

Tdc  —  7  [kndTd  ~  kfj,dc (fid  —  fi  )] 

(3.39) 

Similar  controllers  for  autopilot  compensation  are  designed  for  the  remaining  control  vari- 

ables  as  adc,  Tdc- 
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3.4  Noise-free  Performance  of  the  System 


It  is  mandatory  to  check  the  performance  of  the  system  in  noise-free  situations.  The  ro¬ 
bustness  of  the  system  can  be  verified  in  noisy  conditions  in  the  later  sections.  Both  the 
kinematic  model  as  well  as  point  mass  model  of  UAV  are  considered  for  simulation  studies. 
The  reactive  collision  avoidance  algorithm  is  applied  for  the  UAV  in  a  single  obstacle  scenario 
and  the  performance  of  the  algorithm  is  evaluated. 


3.4.1  Kinematic  Model  Noise-free  Performance:  Stationary  Ob¬ 
stacle 


The  kinematic  model  of  UAV  is  considered  for  reactive  collision  avoidance  in  noise-free 
scenario.  The  simulation  is  carried  out  with  stationary  single  obstacle  the  Initial  Position 
of  UAV  is  [0,0,0],  Initial  Velocity  [19.9,0.97,0.99],  Target  Position  [350,— 5,— 5],  Obstacle 
position  [200, —2.5, —2.5]  and  UAV  final  location  [345.6, —4.0, —4.0].  Fig.  3.2  shows  the 
trajectory  of  the  UAV.  The  guidance  commands  for  the  UAV  with  kinematic  model  is  shown 
in  Fig.  3.3.  The  guidance  for  x  and  7  are  implemented  through  first  order  autopilot  lag 
equations.  Velocity  of  the  UAV  is  kept  constant  throughout  the  simulation  and  aiming  point 
is  achieved  through  variation  of  x  and  7.  Therefore,  UAV  with  kinematic  model  is  able  to 
avoid  the  obstacle  in  noise-free  scenario. 


sphere 

Obstacle 

★  Initial  position 
'fa  final  position 
■  -  relative  position 

- trajectory 


UAV  Estimated  Trajectory 


X  y  and  velocity 


time(sec) 


Figure  3.2:  UAV  Kinematic  model  with  Single  Figure  3.3:  UAV  Kinematic  model  with  Single 
Stationary  Obstacle:  UAV  Trajectory  Stationary  Obstacle:  y,  7  and  Velocity 
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3.4.2  Point  mass  Model  Noise-free  Performance:  Stationary  Ob¬ 
stacle 


The  point  mass  model  of  UAV  is  considered  for  reactive  collision  avoidance  in  noise- 
free  scenario.  This  model  has  a  close  resemblance  with  practical  UAVs  as  the  aerody¬ 
namic  lift  and  drag  force  coefficients  are  taken  into  consideration.  The  simulation  is  carried 
out  with  stationary  single  obstacle  the  Initial  Position  of  UAV  is  [0,0,50],  Initial  Velocity 
[19.9,0.97,0.99],  Target  Position  [350,  —5,52],  Obstacle  Position  [140,  —3,51]  and  UAV  final 
location  [346.18,-4.35,52.29].  Fig.  3.4  shows  the  trajectory  of  the  UAV.  The  guidance 
commands  for  the  UAV  with  point  mass  model  is  shown  in  Fig.  3.5.  The  initial  values 
of  guidance  commands  are  a  =  2.314°  ,  /i  =  0°  and  Thrust  =  5 N .  The  commands  are 
exercised  during  the  collision  avoidance  and  afterwards  the  commands  are  almost  constant 
as  the  destination  is  determined. 


Figure  3.4:  UAV  Point  mass  model  with  Single 
Stationary  Obstacle:  UAV  Trajectory 


Figure  3.5:  UAV  Point  mass  model  with  Single 
Stationary  Obstacle:  /i,  a  and  Thrust 


3.4.3  Point  mass  Model  Noise-free  Performance:  Moving  Obstacle 

The  point  mass  model  of  UAV  is  considered  for  reactive  collision  avoidance  in  noise-free 
scenario.  The  simulation  is  carried  out  with  single  moving  obstacle  with  Velocity  [2,0,0], 
the  Initial  position  of  UAV  is  [0,0,50],  Initial  Velocity  [19.9,0.97,0.99],  Target  Position 
[350,-5,52],  Obstacle  initial  position  [140,-3,51],  Obstacle  final  position  [174.8,-3,51] 
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and  UAV  final  location  [346.63,  —4.3,  52.3].  Fig.  3.6  shows  the  trajectory  of  the  UAV.  Since 
the  obstacle  is  moving,  the  final  location  of  obstacle  is  shown  in  the  trajectory.  The  guidance 
commands  for  the  UAV  with  point  mass  model  is  shown  in  Fig.  3.7.  The  commands  are 
executed  to  avoid  collision  with  the  dynamic  obstacle  as  the  aiming  point  computation  is 
incurred  due  to  relative  motion  caused  by  UAV  as  well  as  the  obstacle  motion. 


UAV  Estimated  Trajectory 
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Figure  3.6:  UAV  Point  mass  model  with  Single 
Moving  Obstacle:  UAV  Trajectory 


Figure  3.7:  UAV  Point  mass  model  with  Single 
Moving  Obstacle:  //,  a  and  Thrust 


Fig.  3.8  shows  the  obstacle’s  true  and  estimated  velocity  profile.  The  estimated  velocity 
through  first  order  differential  of  sensor  dynamics  is  inaccurate  and  fluctuating.  In  later 
sections  the  obstacle  velocity  is  accurately  estimated  using  extended  kalman  filter  as  well 
as  unscented  kalman  filter.  Fig.  3.9  shows  the  minimum  distance  profile.  The  guidance 
commands  for  a  and  x  show  some  fluctuations  at  around  6  to  8  seconds  which  is  due  to 
UAV’s  close  proximity  with  obstacle  safety  sphere  and  invoking  sphere  tracking  algorithm 
[9], 
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True  Estimated  velocity  obstacle(m/s) 


Obstacle  True  and  Estimated  Velocity  (m/s)  Distance  from  UAV  to  OBS  and  Target 


Figure  3.8:  UAV  Point  mass  model  with  Single  Figure  3.9:  UAV  Point  mass  model  with  Single 
Moving  Obstacle:  Velocity  Profile  Moving  Obstacle:  Minimum  Distance  Profile 
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3.5  Extension  of  the  Algorithm  for  Multiple  Stationary 
Obstacles 

Without  loss  of  generality,  multiple  stationary  obstacles  are  considered  here  in  the  problem 
formulation.  The  formulation  can  be  treated  as  ’reconfigurable’  [48]  in  the  sense  that  if 
some  of  the  obstacles  are  in  close  proximity  with  each  other,  then  one  composite  obstacle 
is  considered  encircling  all  of  these  obstacles  rather  than  individual  obstacles.  The  collision 
avoidance  algorithm  is  implemented  considering  only  the  composite  obstacles  and  remaining 
individual  obstacles. 

3.5.1  Formulation  of  Reconfigurable  Obstacles 

Let  considered  the  onboard  sensor  mounted  on  the  UAV  senses  the  N  obstacles.  The 
information  of  position  and  safety  sphere  radius  of  each  obstacle  are  obtained  through  the 
onboard  sensors.  First  all  the  N  obstacles  position  and  safety  radius  are  assigned  in  a  vector 
form  as  X  and  Rv  respectively.  One  of  the  obstacles  is  considered  as  global  obstacle  say  Xog 
and  the  distance  between  this  obstacle  centre  and  remaining  other  N  —  1  obstacles  centres 
are  computed.  If  any  of  the  N  —  1  obstacles  are  in  close  proximity  of  the  global  obstacle  (i.e. 
distance  between  the  two  obstacle  centres  is  less  than  the  summation  of  two  obstacle  safety 
sphere  radius  and  minimum  safety  distance  considered  between  them)  then  one  composite 
safety  sphere  is  formed,  which  encompassing  the  safety  spheres  of  both  global  obstacle  and 
the  close  proximity  obstacle  in  following  manner.  Let  us  assume  that  ith  obstacle  in  obstacle 
position  vector  X(:,i)  is  in  close  proximity  to  global  obstacle,  then  a  unit  vector  along  the 
line  joining  the  centre  of  the  global  obstacle  and  close  proximity  obstacle  is  given  as. 

XQJl-Xoa 
™  ||V(:,i)  —  Xqg\\ 

The  radius  and  centre  of  composite  safety  sphere  are  compute  as  follows 


r  a  =  (rg  +  Rv(i)  +  \\X(:,i)~XOG\\)/2 

Xa  =  Xqg  +  ( ra  -  rg)Xrv  (3.41) 

Where,  Rv[i)  and  rg  are  the  safety  sphere  radius  of  the  close  proximity  and  global  obstacle 
respectively.  Xa  and  ra  are  the  centre  and  radius  of  the  composite  safety  sphere  respectively. 
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There  may  be  situation  such  that  either  one  of  the  obstacle  safety  sphere  is  completely  lie 
inside  the  other  obstacle  safety  sphere.  In  such  scenario  the  computed  ra  comes  either  less 
than  or  equal  to  the  bigger  obstacle  safety  sphere  radius.  Therefore,  choose  the  centre  and  the 
radius  of  the  bigger  obstacle  safety  sphere  as  the  composite  safety  sphere.  This  composite 
safety  sphere  is  updated  as  the  global  obstacle  and  the  process  is  repeated  for  remaining 
obstacles  considering  one  by  one  steps.  The  obstacles  which  are  not  in  close  proximity  to 
the  considered  global  obstacle  are  regarded  as  separate  obstacles.  The  position  and  safety 
radius  information  of  the  separate  obstacles  are  stored  in  a  separate  vector  as  Xsep  and  rsep 
respectively.  Further  this  separate  vector  Xsep  along  with  global  obstacle  is  put  together 
and  it  forms  a  new  set  of  obstacles.  This  method  is  iterated  for  this  new  set  in  one  by  one 
steps  considering  each  one  as  global  obstacle.  Finally,  the  N  obstacle  set  is  transformed  into 
a  new  final  set  say  XNew  which  includes  clustered  global  obstacles  as  well  as  un-clustered 
global  obstacles.  The  entire  logic  is  described  in  Fig.  3.10  in  flowchart  format. 

3.5.2  Collision  Avoidance  algorithm  for  Multiple  Individual  Ob¬ 
stacles 

Initialize  the  UAV  aiming  point  as  a  goal  point  Xap  =  Xgoai  and  corresponding  time-to-go 
as  minimum  time  to  reach  tmm  =  tgo  and  initialize  the  final  set  of  obstacles  XNew.  Then 
compute  the  aiming  point  Xapk,  time  to  reach  the  plane  t*k  and  check  the  collision  criteria 
for  the  kth  global  obstacle  in  new  set  X]yew  using  Eq.(3.16)  to  Eq.(3.18).  If  the  collision 
criteria  is  satisfied  and  computed  t*k  is  lesser  than  tmm  then  update  the  UAV  aiming  point  as 
Xap  =  Xapk  and  the  minimum  time  to  reach  as  tmm  =  t*k.  The  above  procedure  is  repeated 
for  remaining  all  global  obstacles  in  the  new  set  X^ew  one  by  one.  More  detailed  logic  is 
given  in  flowchart  as  shown  in  Fig.  3.11 


41 


Initialize 


#  Collision  criteria  is  evaluated  using  Eq.  (21)  in  Sec  III.  A 


Figure  3.11:  Flowchart  for  Collision  Avoidance  of  Multiple  Individual  Obstacles 


Chapter  4 

Obstacle  Position  and  Size  Estimation 
with  Camera 


In  the  previous  section,  assumption  is  made  that  the  obstacle  is  point  obstacle  and  size 
of  the  obstacle  considering  a  safety  radius  around  it  was  assumed  spherical.  The  camera 
coordinates  of  the  obstacle’s  projection  on  cameras  image  plane  are  obtained  with  onboard 
camera  sensors  mounted  on  the  UAV  and  image  processors.  In  this  section,  the  procedure 
involved  in  obtaining  the  pixel  data  on  image  plane  and  extracting  the  obstacle  information 
is  described  to  make  a  closer  resemblance  with  realistic  approach.  The  pin  hole  camera 
model  is  considered  and  assumption  made  here  is  that  camera  is  ideal  and  calibrated.  The 
shape  of  the  obstacle  is  considered  as  sphere.  The  steps  followed  can  be  summerised  as 
firstly,  Extracting  the  potential  feature  points  from  stereo  images  using  harris  detector  and 
then,  stereo  matching  is  done  along  epipolor  line.  Finally,  the  3D  information  of  obstacle 
obtained  by  applying  triangulation  method  to  the  matched  feature  points. 

4.1  Camera  Model 

In  this  work,  the  pinhole  camera  model  is  used  for  describing  the  geometric  relations  between 
3D  object  and  their  projections  onto  the  image  plane  of  the  camera  and  its  geometry  is  shown 
in  Fig  4.1  [46].  The  camera  axis  system,  denoted  with  subscript  c,  is  defined  to  have  its  origin 
at  the  camera  centre  C  with  the  XCYC  plane  parallel  to  the  image  plane.  The  intersection 
point  of  the  Zc  axis  in  image  plane  is  called  the  principal  point  which  is  close  to  the  centre  of 
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Figure  4.1:  The  camera  coordinate  and  world  coordinate  frame 
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the  image  plane.  The  focal  length  /  is  defined  as  the  distance  between  the  camera  centre  and 
the  image  plane.  The  mathematical  relation  between  the  camera  coordinate  to  the  projected 
image  (pixel)  coordinate  is  given  by  a  camera  calibration  matrix  K  as  follows  [46], 


sh  0  0 

1 - 

O 

K  = 

0  0 

0  fy  Pv 

0  0  1 

0  0  1 

K 


®-h  0  Ch 

0  CVy  Cy 

0  0  1 


(4.2) 


where,  s h,  sv  are  pixel  size  in  the  horizontal  and  vertical  direction  of  the  image  respec¬ 
tively,  K  is  also  called  as  the  intrinsic  matrix,  and  av  represent  the  focal  length  (in  pixels ) 
in  the  horizontal  and  vertical  direction  respectively.  Ch,  cv  are  the  image  centre. 

let  us  define  the  world  coordinate  system  with  subscript  w.  To  incorporate  the  camera 
position  and  orientation  with  respect  to  world  coordinate  system,  we  rotate  and  translate 
the  point  Xc  in  the  camera  coordinate  system.  In  Euclidean  coordinates  this  will  give  us 


Xc  =  R(XW  -  C)  (4.3) 

where,  R  is  a  3  x  3  rotation  matrix  and  C  is  the  position  of  the  camera  centre  in  Euclidean 
coordinates.  The  relation  between  world  coordinate  to  camera  coordinate  can  be  represented 
in  homogeneous  coordinates  as  follows, 


xc 

xw 

yC 

’  R  — R  C 

yW 

zc 

°T  1 

zw 

1 

1 

The  camera  matrix  (it  is  also  called  projection  matrix)  is  given  as, 

P  =  KR[I,  -C\ 


(4.4) 


(4.5) 
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4.2  Stereo  Camera 


A  stereo  vision  system  is  composed  of  two  pinhole  cameras,  a  3D  object  and  its  projection  on 
both  image  planes  shown  in  Fig.  4.2.  Using  two  or  more  images  of  the  scene,  each  acquired 
from  a  different  viewpoint  in  space,  feature  detection  and  stereo  matching  of  3D  structure  is 
carried  out  by  triangulation  [21]  method. 

4.2.1  Feature  detection 

The  detection  of  feature  points  is  the  determination  of  interest  points  in  a  given  im¬ 
age  frame.  Various  methods  for  finding  interest  points  are  available,  some  of  them  are  the 
Scale-Invariant  Feature  Transform  (SIFT)  [23,  24],  the  Speeded  Up  Robust  Features  (SURF) 
detector  [25],  Feature  form  accelerated  segment  test  (FAST)  [26]  and  the  Harris  corner  [28]. 
The  SIFT  feature  points  are  computed  by  finding  the  potential  feature  in  different  scale 
space.  These  features  are  localized  upto  the  subpixel  accuracy  and  are  given  in  the  form 
of  descriptor  vector.  Each  descriptor  consists  of  scale  and  orientation  of  the  corresponding 
feature.  SIFT  features  are  very  accurate  and  it  is  invariant  to  scale,  rotation,  projective 
transformation  and  illumination  changes  but  it’s  slow  to  execute  and  computationally  in¬ 
tensive.  In  SURF  detector  is  computationally  fast  and  less  accurate  when  it’s  compared  to 
SIFT.  It  uses  hessian  matrix  which  can  be  calculated  using  integral  images.  In  this  report 
we  have  used  the  Harris  corner  detector  [28],  compared  to  remaining  methods  it  is  simple 
and  less  computational  complexity. 

Harris  corner  detection  is  used  to  extract  a  set  of  feature  points  from  the  left  and  right 
images.  An  interest  operator  tuned  for  corner  detection  is  applied  to  image  pair  and  pixels 
with  the  highest  interest  values  selected  as  features.  These  potential  features  are  also  used 
for  calibration  of  stereo  camera.  In  this  method  involves  shifting  a  small  patch  or  window 
of  image  in  all  directions.  If  the  window  contains  any  corner  point  then  shifting  the  window 
along  all  direction  results  in  large  change  in  its  intensity  values. 

E(u,  v)  ^  ^  IUj:,y  | Iu+x,v+y  ^x,y\  (4.6) 

%,y 

IX)V  intensity  value  at  point  ( x,y ),  u,v  introduce  a  small  shift  amount  of  the  window. 
The  window  is  chosen  as  a  gaussian  window  Wx.y.  Using  the  Taylor  series,  the  expression 
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Iu+X)V+y  in  above  equation  can  be  rewritten  as, 


E(u,  v')  ^  ^  | l-x,y  T  "F  -^c,y| 

x,y 


(4.7) 


E(u,v)  =  [u,v]M 


u 

v 


where  M  is  called  covariance  matrix, 


(4.8) 


M  =  J2  w-.y 

x,y 


Ix,x  Ix,y 

I%,y  Iy,y 


The  quality  of  the  corner  can  be  measured  by  the  following  response  function, 


(4.9) 


R  =  Det(M)  -  K(Trace(M ))2  (4.10) 

The  response  function  R  only  depends  on  the  eigen  values  of  M.  For  corner  points  it  will 
produce  the  large  positive  value  (R  >  0),  for  edge  region  it  will  give  the  large  negative  value 
(R  <  0)  and  for  flat  region  it  will  give  small  |i?|  value. 


4.2.2  Stereo  matching 

Stereo  matching  algorithm  is  used  to  find  correspondences  between  the  extracted  feature 
points  between  left  and  right  images  which  gives  relative  position  and  orientation  between 
stereo  cameras  (i.e.  Fundamental  matrix). 

4.2.3  Epipolar  constraint 

In  this  section,  the  epipolar  geometry  [21]  is  presented.  Consider  two  image  planes  with  the 
distinct  viewpoints  as  in  Fig.  4.2.  Let  Xi,  yi  and  xr,  yr  be  the  corresponding  feature  points  in 
two  image  planes.  The  epipolar  geometry  defines  the  geometric  relationship  between  these 
corresponding  points.  The  plane  passing  through  the  camera  center  C\  and  Cr  and  the  3D 
world  point  xw  is  called  an  epipolar  plane.  The  projection  e(e')  of  one  camera  center  onto 
the  image  plane  of  the  other  camera  frame  is  called  an  epipole.  An  epipolar  line  l(V)  is  the 
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Figure  4.2:  The  geometry  of  stereo  camera 


intersection  of  an  epipolar  plane  for  xw  with  the  image  plane.  All  epipolar  lines  pass  through 
the  epipole. 

4.2.4  Fundamental  Matrix 

The  fundamental  matrix  represents  the  epipolar  geometry  algebraically  and  contains  most 
of  the  information  about  the  relative  position  and  orientation  between  the  two  views.  If  F 
is  the  fundamental  matrix  [42,  44],  then  FT  is  the  matrix  which  satisfies 

X'tFX  =  0  (4.11) 

for  all  corresponding  points  x  and  x'  in  both  left  and  right  image  planes.  For  any  point  x 
in  the  first  image,  the  corresponding  epipolar  line  is  V  =  Fx  .  Similarly,  l  =  FTx'  represents 
the  epipolar  line  corresponding  to  x'  in  the  second  image  plane.  For  any  point  x  (other  than 
epipole  e  ),  the  epipolar  line  V  =  Fx  contains  the  epipole  e' .  Thus  e'  satisfies  F1  Fx  =  0  for 
all  x. 

In  this  work,  we  have  used  SSD-based  matching  by  searching  both  the  images  to  minimize 
the  similarity  criterion.  The  similarity  criteria  can  be  represented  by, 

SSD(x,y)=  £  (Iieft(iJ)-Iright(x  +  i,y  +  j))2  (4.12) 

( i,j)eW 
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The  stereo  matching  is  done  strictly  along  the  epipolar  line  with  only  a  few  pixels  to 
offset  above  and  below  it.  RANSAC  [22], [27]  is  used  to  reject  outliers. 


4.3  3D  Reconstruction 

The  relationship  between  a  point  in  the  scene  and  its  corresponding  point  in  the  camera  and 
the  projector  image  coordinates  can  be  written  for  left  and  right  images  as  follows  [47], 
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(4.13) 


(4.14) 


x =  Pxw,  x?  =  Qxw 

Where,  P  and  Q  denote  the  projection  matrix  of  the  camera  and  the  projector  respec¬ 
tively,  and  the  superscripts  p 1  and,  p 2  mean  the  pixel  coordinate  frames  of  the  camera  and 
wl  and  w2  denotes  world  frame  projector  respectively.  If  a  pair  of  corresponding  points  in 
two  images  can  be  found,  it  suggests  that  they  are  the  projection  of  a  common  3D  point. 
This  can  be  Reconstruction  by  Singular  Value  Decomposition  (SVD)  or  inverse  pseudo  spec¬ 
tral  method.  In  our  work  SVD  is  used  for  finding  object  points.  Finally  it  (Eqn  4.13  and 
4.14)  is  expressed  in  terms  of  homogeneous  coordinates. 


Mxw  =  0 


Where  M  = 


m  11 

m  12 

m  i3 

ml4 

m2 1 

m2  2 

m23 

m2  4 

m3 1 

m3  2 

m33 

m34 

m4 1 

m42 
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m44 
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(4.15) 


mn  =  pn  -  xplp31  , 
ml3  =  pi3  -  xplp33  , 
m2i  =  P21  -  yp'p:n  , 
m23  =  P23  -  VplP33  , 
m3 1  =  gn  -  xp2  q33  , 
m33  =  q13  -  xp2q33  , 
wi4i  =  921  -  l/p293i  , 

™43  =  <?23  -  l/p2933  , 


m12  =  P12  -  a;pip32 
mM  =  P14  -  a;pip34 
m22  =  p22  -  yplp32 
m24  =  p24  -  yplp34 
m32  =  q12  -  xpZ  q32 
m33  =  qu  ~  xpZ  q3 4 
m42  =  q2 2  -  ypZq3 2 
m44  =  q2 4  -  yp2g34 


Thus,  we  can  estimate  the  point  through  singular  value  decomposition  (SVD)  related 
techniques.  The  four  elements  of  the  last  column  of  V  obtained  by  SVD  of  M  ( UDVT )  are 
the  homogeneous  coordinates  of  xw. 


4.4  Simulation  Result 


In  this  section,  point  mass  model  of  UAV  is  considered.  Simulations  are  carried  out  in 
Matlab’s  VRML  environment.  For  simplicity,  obstacle  shape  with  safety  radius  is  taken 
as  spherical.  Virtual  stereo  camera  is  mounted  on  UAV  in  VRML  environment.  The  base 
length  B  between  two  cameras  is  taken  as  30cm.  Field  of  view  of  horizontal  and  vertical 
direction  is  taken  as  fov  =  60°.  Resolution  of  the  virtual  camera  is  576  x  380  and  the  focal 
length  (in  pixel)  can  be  computed  using  following  expression. 


®-h  fh^h 


Oiy 


fv 


0.5  *  width  576 

-  - 

tan(fov  *  0.5)  width 

(4.16) 

0.5  *  height  380 

-  - 

tan(fov  *  0.5)  height 

(4.17) 

The  camera  center  is  taken  as  Ch= 288,  c„=190. 


4.4.1  Single  Obstacle 

The  simulation  is  carried  out  with  stationary  single  obstacle  and  the  initial  position  of 
UAV  is  [20,10,10],  Initial  Velocity  is  [10,1.2,1.0],  Target  Position  is  [150,22,22],  The  fea¬ 
ture  points  detection  algorithm  results  are  shown  in  Fig.  4.3.  The  stereo  matching  is  done 
by  searching  the  minimum  sum  of  square  difference  (SSD)  of  potential  features  along  the 
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epipolor  line  and  outliers  are  removed  by  RANSAC  algorithm  shown  in  Figs.  4.4.  The  3D- 
Reconstruction  of  the  potential  features  are  shown  in  Fig.  4.5.  The  approximate  value  of 
the  obstacle  centre  can  be  obtained  by  finding  the  mean  of  the  reconstructed  3D-potential 
feature  points.  However  obstacle  centre  may  not  be  an  exact  one  but  due  to  3D  symmet¬ 
rical  nature  of  the  sphere  the  calculation  of  centre  is  closed  to  the  actual  one.  Radius  of 
the  sphere  can  be  computed  by  taking  average  distance  between  computed  centre  and  all 
feature  points.  Moreover,  if  the  UAV  is  approaching  closer  to  the  obstacle  it  may  get  only 
partial  information  about  the  obstacle  because  of  the  restriction  of  its  field  of  view  .  To 
avoid  this  problem  the  value  of  the  obstacle  centre  and  radius  is  freezed  from  some  threshold 
distance  between  UAV  and  the  computed  obstacle.  In  this  simulation  we  got  obstacle  centre 
as  [110.46, 14.52,6.62]  and  radius  is  15.15m. 


Figure  4.3:  Feature  points  detected  by  Harris 
corner 
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Figure  4.4:  SSD  based  stereo  matching 


The  3D  trajectory  of  UAV  is  shown  in  Fig.  4.6.  The  guidance  command  such  as  Angle  of 
attack  and  Bank  angle  profile  is  shown  in  Figs  4.7  and  4.8  respectively.  The  Thrust  profile 
is  shown  in  Fig.  4.9 
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mu  (degree)  g  displacement  along  Z 


Figure  4.5:  3D  reconstruction  of  matched  feature  points 


Figure  4.6:  UAV  Trajectory 


time  (s) 


Figure  4.7:  Angle  of  attack  Profile 


Figure  4.8:  Bank  angle  Profile 


Figure  4.9:  Thrust  Profile 
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Chapter  5 

Simulation  Results 


5.1  Simulation  Results 

Simulations  are  carried  out  to  test  the  performance  of  reactive  collision  avoidance  algo¬ 
rithm.  Both  the  Kinematic  model  and  Point  mass  model  of  UAV  is  considered  for  the  system 
dynamics.  The  aerodynamic  data  for  the  model  is  taken  from  the  AE-2  model  of  the  UAV 
as  shown  in  Fig.A.l.  Single  stationary  obstacle,  single  moving  obstacle  with  constant  veloc¬ 
ity  and  multiple  stationary  obstacles  are  considered  for  simulation  study.  Validation  of  the 
reactive  collision  avoidance  algorithm  is  also  carried  out  with  randomized  simulations.  For 
extracting  the  accurate  data  from  noisy  sensor  measurements  EKF  as  well  as  UKF  estima¬ 
tion  techniques  are  used  which  estimate  the  obstacle  position  as  well  as  obstacle  velocity.  In 
order  to  have  similarity  with  practical  stereovision  sensors  the  separation  between  the  two 
sensors  is  0.40  meter  and  focal  length  is  0.22  centimeter  is  considered  for  both  the  sensors. 

5.2  Kinematic  model  of  UAV  with  EKF  Estimation 

In  this  section  Kinematic  model  of  UAV  is  considered  and  camera  sensor  model  is  assumed 
to  be  noisy  and  the  camera  sensor  state  estimation  is  carried  out  using  Extended  Kalman 
Filter.  The  simulation  is  carried  out  with  single  stationary  as  well  as  single  moving  obstacle 
with  constant  velocity.  The  objective  here  is  to  compute  the  estimated  position  of  the 
obstacle  and  use  the  guidance  command  to  avoid  the  collision  with  in  the  available  time  to 
go  and  guide  the  UAV  to  appropriate  destination. 


54 


UAV  Estimated  Trajectory 


time(sec) 


Figure  5.1:  UAV  Kinematic  model  with  EKF  Figure  5.2:  UAV  Kinematic  model  with  EKF 
Single  Stationary  Obstacle:  UAV  Trajectory  Single  Stationary  Obstacle:  y,  7  and  Velocity 


5.2.1  Single  obstacle 

Case  1:  Stationary  Obstacle 

The  simulation  is  carried  out  with  single  stationary  obstacle  the  initial  position  of  UAV  is 
[0,0,0],  Initial  Velocity  [19.33,-4.5,3.3],  Target  Position  [279,-6.3,9.2],  Obstacle  position 
[138,-7.0,4.0]  and  UAV  final  location  [274,-7.4,10.0].  Kinematic  model  of  UAV  is  con¬ 
sidered  and  Extended  Kalman  Filter  is  used  for  sensor  state  estimation.  Fig.  5.1  shows 
the  trajectory  of  the  UAV.  The  guidance  commands  for  the  UAV  with  kinematic  model  is 
shown  in  Fig.  5.2.  Autopilot  lag  compensation  is  provided  for  the  guidance  command  and 
corresponding  trajectory  is  also  plotted  which  almost  overlaps  the  actual  trajectory.  Velocity 
profile  of  the  UAV  is  constant  throughout  the  simulation.  The  guidance  commands  y  and  7 
are  tracking  the  desired  guidance  command. 

The  sigma  error  bounds  for  stereovision  camera  sensor  states  r,  y  and  7  are  given  as  Fig. 
5.3.  The  plot  shows  that  errors  are  within  the  bounds.  The  UAV  to  target  distance  and  the 
UAV  to  obstacle  distance  is  shown  in  Fig.  5.4  where  the  UAV  to  obstacle  distance  is  firstly 
decreasing  and  further  increasing  as  the  UAV  first  approaches  towards  the  obstacle  and  then 
avoids  it  by  executing  the  guidance  cammand  and  moves  towards  the  destination.  However, 
the  distance  to  target  is  constantly  decreasing.  Simulation  stopping  criteria  is  applied  when 
the  UAV  reaches  within  5 m  range  of  the  destination. 

Case  2:  Moving  Obstacle 
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OBSTACLE:!  -Sigma-Bound 


EKF-Prestart-to-end-time(sec) 


50 


-50 1 - 1 - 1 - 1 

0  5  10  15 

EKF-Prestart-to-end-time(sec) 

Figure  5.3:  UAV  Kinematic  model  with  EKF 
Single  Stationary  Obstacle:  Sigma  error  bound 


Distance  from  UAV  to  OBS  and  Target 


Figure  5.4:  UAV  Kinematic  model  with  EKF 
Single  Stationary  Obstacle:  UAV  to  Obstacle 
and  Target  Distance 


The  simulation  is  carried  out  with  single  obstacle  moving  with  constant  velocity  [2, 0,  0],  the 
Initial  position  of  UAV  is  [0,  0, 0],  UAV  Initial  Velocity  is  [19.9,  0.97,  0.99],  Target  Position  is 
[389.9,  —3.0,  —3.0],  Obstacle  initial  position  is  [120.0,  —0.54,  —0.51],  Obstacle  initial  position 
is  [159.10,-0.54,-0.51]  and  UAV  final  location  is  [386.9, —2.6, —2.7].  Kinematic  model  of 
UAV  is  considered  and  extended  kalman  filter  is  used  for  estimation.  The  objective  here  is  to 
compute  the  estimated  position  as  well  as  the  estimated  velocity  of  the  obstacle  and  use  the 
guidance  command  for  the  reactive  collision  avoidance  with  the  obstacle  in  the  available  time 
to  go.  In  the  moving  scenario  the  obstacle  is  considered  non-cooperative  and  the  concept 
of  miss  distance  [7]  is  used.  Fig.  5.5  shows  the  trajectory  of  the  UAV.  The  trajectory  with 
autopilot  compensation  is  also  plotted.  The  UAV  is  able  to  avoid  collision  with  moving 
obstacle. 
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UAV  Estimated  Trajectory 


Figure  5.5:  UAV  Kinematic  model  with  EKF  Single  Moving  Obstacle:  UAV  Trajectory 

The  guidance  commands  for  the  UAV  with  kinematic  model  is  shown  in  Fig.  5.7.  The 
guidance  command  is  tracking  the  desired  command.  The  following  Fig.  5.6  shows  autopilot 
compensation  provided  for  guidance  command.  Sigma  error  bounds  for  stereovision  camera 
sensor  states  are  given  as  Fig.  5.8  which  are  well  with  in  bounds.  The  UAV  to  target  distance 
and  the  UAV  to  obstacle  distance  profile  is  shown  in  Fig.  5.9 

The  obstacle  is  assumed  to  be  moving  with  constant  velocity  and  based  on  successive 
data  obtained  from  stereovision  camera  sensors  in  their  coordinate  frames,  using  camera 
sensor  state  dynamics  and  using  the  Extended  Kalman  Filter  estimation,  the  velocity  of  the 
obstacle  is  estimated  and  as  shown  in  the  Fig. 5. 10.  The  velocity  estimate  becomes  erroneous 
as  the  camera  sensor  comes  to  closer  to  obstacle.  This  can  be  correlated  with  the  time 
instants  shown  in  the  fig. 5. 11  where  the  forward  direction(x-direction)  position  components 
of  UAV  and  obstacle  are  plotted. 

ft  is  observed  that  the  guidance  command  is  efficiently  able  to  perform  the  reactive 
collision  avoidance  in  single  obstacle  scenario  when  the  obstacle  is  stationary  as  well  as 
moving  with  constant  velocity. 
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time(sec) 


Figure  5.6:  UAV  Kinematic  model  with  EKF 
Single  Moving  Obstacle:  Autopilot  Compensa¬ 
tion 


Figure  5.7:  UAV  Kinematic  model  with  EKF 
Single  Moving  Obstacle:  \i  7  and  Velocity 


5.3  Kinematic  model  of  UAV  with  UKF  Estimation 

In  this  section  Kinematic  model  of  UAV  is  considered  and  camera  sensor  model  is  as¬ 
sumed  noisy  and  the  camera  sensor  state  estimation  is  carried  out  using  Unscented  Kalman 
Filter(UKF).  The  UKF’s  excellence  over  EKF  for  nonlinear  systems  in  noisy  conditions  are 
exploited  and  the  simulation  results  are  presented  for  single  stationary  obstacle  environment. 
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OBSTACLE:!  -Sigma-Bound 
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Figure  5.8:  UAV  Kinematic  model  with  EKF 
Single  Moving  Obstacle:  Sigma  error  bound 


True  and  Estimated  Velocity(m/s) 


Figure  5.10:  UAV  Kinematic  model  with  EKF 
Single  Moving  Obstacle:  True  and  Estimated 
Velocity  of  Obstacle 


Distance  from  UAV  to  OBS  and  Target 


Figure  5.9:  UAV  Kinematic  model  with  EKF 
Single  Moving  Obstacle:  UAV  to  Obstacle  and 
Target  Distance 


Obs  and  UAV  Position  in  X-direction  (m) 


Figure  5.11:  UAV  Kinematic  model  with  EKF 
Single  Moving  Obstacle:  Obstacle  and  UAV’s 
X-direction  Position 
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Figure  5.12:  UAV  Kinematic  model  with  UKF 
Single  Stationary  Obstacle:  UAV  Trajectory 


Figure  5.13:  UAV  Kinematic  model  with  UKF 
Single  Stationary  Obstacle:  y,  7  and  Velocity 


5.3.1  Single  obstacle 

The  simulation  is  carried  out  with  single  stationary  obstacle  the  initial  position  of  UAV  is 
[0,  0,  0],  Initial  Velocity  [19.95, 0.97,  0.99],  Target  Position  [319.9,  0.65,  3.3],  Obstacle  position 
[123.85, 1.21,0.78]  and  UAV  final  location  [316.35, 1.16,4.0]  kinematic  model  of  UAV  is  con¬ 
sidered  and  Unscented  kalman  filter  is  used  for  estimation.  Fig.  5.12  shows  the  trajectory 
of  the  UAV  with  actual  guidance  as  well  as  guidance  with  autopilot  compensation.  The 
guidance  commands  for  the  UAV  with  kinematic  model  is  shown  in  Fig.5.13  where  tracking 
of  the  desired  guidance  command  is  shown.  The  velocity  of  the  UAV  is  considered  to  be 
constant  throughout  the  simulation. 

The  autopilot  compensation  for  lag  in  guidance  command  is  provided  in  Fig.5.14.  The 
UAV  to  target  distance  and  the  UAV  to  obstacle  distance  is  shown  in  Fig.  5.15.  It  is  observed 
that  the  guidance  command  is  efficiently  able  to  perform  the  collision  avoidance  in  single 
stationary  obstacle  scenario  with  kinematic  model  of  UAV  and  with  UKF  estimation. 


60 


X  y  and  vel 


time(sec) 


Distance  from  UAV  to  OBS  and  Target 


8.5  cm 


Figure  5.14:  UAV  Kinematic  model  with  UKF 
Single  Stationary  Obstacle:  Autopilot  Compen¬ 
sation 


Figure  5.15:  UAV  Kinematic  model  with  UKF 
Single  Stationary  Obstacle:  UAV  to  Obstacle 
and  Target  Distance 
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5.4  Point  mass  model  of  UAV  with  EKF  Estimation 


In  this  section  Point  mass  model  of  UAV  is  considered  and  stereovision  camera  sensor 
model  is  assumed  noisy  and  the  camera  sensor  state  estimation  is  carried  out  using  Ex¬ 
tended  Kalman  Filter.  The  simulation  is  carried  out  with  single  stationary  obstacle,  single 
moving  obstacle  with  constant  velocity  as  well  as  multiple  stationary  obstacle.  The  objective 
here  is  to  compute  the  estimated  position  of  the  obstacle  as  well  as  estimated  velocity  of  the 
obstacle  (for  moving  obstacle  only)  and  use  the  guidance  command  to  avoid  the  collision  in 
the  available  time  to  go. 

5.4.1  Single  obstacle 

Case  1:  Stationary  Obstacle 

The  simulation  is  carried  out  with  single  stationary  obstacle  the  initial  position  of  UAV 
is  [0,0,50],  Initial  Velocity  [19.95,0.97,0.99],  Target  Position  [267.0,-4.0,46.0],  Obstacle 
position  [169.95,  —1.50,48.40]  and  UAV  Final  location  [266.6,  —2.4,46.0].  Point  mass  model 
of  UAV  is  considered  and  Extended  Kalman  Filter  is  used  for  estimation.  Fig.  5.16  shows 
the  trajectory  of  the  UAV  obtained  with  actual  guidance  command  as  well  as  guidance  with 
autopilot  compensation.  The  guidance  commands  for  the  UAV  with  point  mass  model  is 
shown  in  Fig.  5.17  where  the  desired  guidance  command  is  computed  through  the  aiming 
point  and  actual  guidance  tracks  the  desired  guidance  command.  Keeping  velocity  constant 
the  angle  commands  are  used  to  achieve  the  aiming  point. 
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UAV  Estimated  Trajectory  X  Y  and  velocity 


Figure  5.16:  UAV  Point  mass  model  with  EKF  Figure  5.17:  UAV  Point  mass  model  with  EKF 
Single  Stationary  Obstacle:  UAV  Trajectory  Single  Stationary  Obstacle:  y,  7  Velocity 
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Figure  5.18:  UAV  Point  mass  model  with  EKF 
Single  Stationary  Obstacle:  /i,  a  and  Thrust 


Figure  5.19:  UAV  Point  mass  model  with  EKF 
Single  Stationary  Obstacle:  Autopilot  Compen¬ 
sation 


The  physical  guidance  commands  a,  ji  and  thrust  which  are  in  turn  computed  using 
the  guidance  commands  7  and  velocity  is  shown  in  Fig.  5.18.  Since  the  UAV  model 
is  an  aerodynamic  model  the  physical  guidance  commands  are  more  relevant.  The  actual 
guidance  command  closely  tracks  the  commanded  value.  The  autopilot  compensation  for 
lag  in  guidance  command  is  shown  in  Fig.  5.19.  The  UAV  to  target  distance  and  the  UAV 
to  obstacle  distance  is  shown  in  Fig.  5.21.  The  sigma  error  bounds  for  stereovision  camera 
sensor  states  are  given  as  Fig. 5. 20. 

The  simulation  results  show  that  UAV  with  point  mass  model  is  able  to  perform  the 
reactive  collision  avoidance  with  the  stationary  obstacle. 
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Figure  5.20:  UAV  Point  mass  model  with  EKF 
Single  Stationary  Obstacle:  Sigma  error  bound 


Figure  5.21:  UAV  Point  mass  model  with  EKF 
Single  Stationary  Obstacle:  UAV  to  Obstacle 
and  Target  Distance 
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UAV  Estimated  Trajectory 


Figure  5.22:  UAV  Point  mass  model  with  EKF  Single  Moving  Obstacle:  UAV  Trajectory 

Case  2:  Moving  Obstacle 

The  simulation  is  carried  out  with  single  obstacle  moving  with  constant  velocity.  Obstacle 
velocity  [2,0,2],  the  Initial  position  of  UAV  is  [0,0,50],  Initial  Velocity  [19.95,0.97,0.99], 
Target  Position  [269.95,-4.0,46.0],  Obstacle  initial  position  [169.95,-1.52,48.4],  Obstacle 
final  position  [197.7,-1.52,48.49]  and  UAV  Final  location  [266.80,-2.50,46.0].  Point  mass 
model  of  UAV  is  considered  and  extended  kalman  filter  is  used  for  stereovision  camera  state 
estimation.  Fig.  5.22  shows  the  trajectory  of  the  UAV  obtained  with  guidance  command  as 
well  as  guidance  with  autopilot  compensation.  The  guidance  commands  for  the  UAV  with 
point  mass  model  is  shown  in  Fig.  5.23  where  the  actual  guidance  command  is  tracking  the 
desired  command.  Since  it  is  being  a  moving  obstacle  scenario  and  the  trajectory  plot  shows 
only  the  final  location  of  the  obstacle,  the  perturbation  in  the  commanded  guidance  is  due 
to  the  UAV’s  close  proximity  with  the  obstacle  safety  sphere. 

The  physical  guidance  command  is  shown  in  Fig.  5.24  which  is  showing  the  a,  p  and 
thrust  profiles  for  averting  the  collision  with  a  moving  obstacle.  The  autopilot  compensation 
for  the  physical  guidance  command  is  shown  in  Fig.  5.25.  The  UAV  to  target  distance  and 
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Figure  5.23:  UAV  Point  mass  model  with  EKF  Figure  5.24:  UAV  Point  mass  model  with  EKF 
Single  Moving  Obstacle:  y,  7  Velocity  Single  Moving  Obstacle:  /1,  a  and  Thrust 

the  UAV  to  obstacle  distance  profiles  are  shown  in  Fig.  5.26. 
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Figure  5.25:  UAV  Point  mass  model  with  EKF  Figure  5.26:  UAV  Point  mass  model  with  EKF 
Single  Moving  Obstacle:  Autopilot  Compensa-  Single  Moving  Obstacle:  UAV  to  Obstacle  and 
tion  Target  Distance 

The  obstacle  is  assumed  to  be  moving  with  constant  velocity  and  based  on  successive 
data  obtained  from  stereovision  camera  sensors  in  respective  camera  coordinate  frames,  using 
sensor  state  dynamics  and  with  the  extended  kalrnan  filter  estimation,  the  velocity  of  the 
obstacle  is  estimated  and  as  shown  in  the  Fig. 5. 27.  The  velocity  estimate  becomes  erroneous 
as  the  camera  sensor  comes  to  closer  to  obstacle.  This  can  be  correlated  with  the  time 
instants  shown  with  Fig.5.28  where  the  forward  direction (x-direction)  position  components 
of  UAV  and  obstacle  are  plotted. 

The  simulation  results  show  that  UAV  with  point  mass  model  dynamics  is  able  to  perform 
reactive  collision  avoidance  in  moving  obstacle  scenario. 
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Obstacle  True  and  Estimated  Velocity  (m/s) 


Figure  5.27:  UAV  Point  mass  model  with  EKF 
Single  Moving  Obstacle:  Obstacle  True  and  Es¬ 
timated  Velocity 


Obstacle  and  UAV  Position  in  X-dir(m) 


Figure  5.28:  UAV  Point  mass  model  with  EKF 
Single  Moving  Obstacle:  Obstacle  and  UAV  Po¬ 
sition  in  x-direction 
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UAV  Estimated  Trajectory 
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Figure  5.29:  UAV  Point  mass  model  with  EKF  Multi  Stationary  Obstacle:  UAV  Trajectory 

5.4.2  Multiple  obstacle 

In  order  to  test  the  performance  of  the  reactive  collision  avoidance  algorithm  in  multiple 
stationary  obstacle  scenario,  the  simulation  is  carried  out  with  three  stationary  obstacles 
with  Initial  positions  [89.97,-2.5,83.49],  [179.97,7.48,84.49]  and  [299.97,12.48,90.49],  The 
initial  position  of  UAV  is  [0,0,50],  Initial  Velocity  of  UAV  [19.95,0.97,0.99],  Target  Position 
[362.97,-4.51,88.49],  UAV  final  position  [359.57,-4.44,86.40].  Point  mass  model  of  UAV  is 
considered  with  EKF  Estimation.  Fig.  5.29  shows  the  trajectory  of  the  UAV  obtained 
through  actual  guidance  command  as  well  as  guidance  with  autopilot  compensation.  In 
order  to  provide  a  clear  view  of  UAV’s  closest  approach  with  the  three  obstacles,  the  UAV 
to  target  distance  and  the  UAV  to  three  different  obstacles  distance  is  shown  in  Fig.  5.30. 
The  guidance  commands  for  the  UAV  with  point  mass  model  is  shown  in  Fig.  5.31  with 
actual  and  commanded  guidance  profiles  for  Xil  and  velocity. 
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Distance  from  UAV  to  Obstacles  and  Target 


Figure  5.30:  UAV  Point  mass  model  with  EKF  Multi  Stationary  Obstacle:  UAV  to  Obstacles 
and  Target  Distance 
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Figure  5.31:  UAV  Point  mass  model  with  EKF  Figure  5.32:  UAV  Point  mass  model  with  EKF 
Multi  Stationary  Obstacle:  y.  7  and  Velocity  Multi  Stationary  Obstacle:  //.  a  and  Thrust 
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Figure  5.33:  UAV  Point  mass  model  with  EKF 
Multi  Stationary  Obstacle:  Autopilot  Compen¬ 
sation 


Figure  5.34:  UAV  Point  mass  model  with  EKF 
Multi  Stationary  Obstacle:  Sigma  error  bound 
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The  physical  guidance  commands  a ,  /i  and  thrust  actual  and  commanded  profiles  is  shown 
in  Fig.  5.32.  The  autopilot  compensation  is  shown  in  Fig.  5.33.  It  can  be  observed  that 
guidance  with  autopilot  compensation  tries  to  minimize  the  lag  with  the  desired  guidance 
command.  The  sigma  error  bound  with  respect  to  obstacle  is  shown  in  Fig.  5.34,  the  camera 
sensor  states  are  well  within  bounds. 

It  is  observed  that  the  guidance  command  is  efficiently  able  to  perform  the  reactive 
collision  avoidance  in  single  stationary  obstacle, single  moving  obstacle  as  well  as  multiple 
stationary  obstacles  scenario  in  the  noisy  sensor  environments  with  EKF  estimation  of  cam¬ 
era  sensor  states. 


5.5  Point  mass  model  of  UAV  with  UKF  Estimation 

In  this  section  Point  mass  model  of  UAV  is  considered  and  camera  sensor  model  is  assumed 
noisy  and  the  stereovision  camera  sensor  states  estimation  is  carried  out  using  Unscented 
Kalman  Filter(UKF).  The  UKF  excellence  over  EKF  for  nonlinear  systems  in  noisy  condi¬ 
tions  are  exploited  and  the  results  are  presented  for  single  moving  obstacle  with  constant 
velocity  as  well  multiple  stationary  obstacles  environments. 

5.5.1  Single  obstacle 

The  simulation  is  carried  out  with  single  obstacle  moving  with  constant  velocity.  Obstacle 
velocity  [2,0,0],  the  Initial  position  of  UAV  is  [0,0,50],  Initial  Velocity  [19.95,0.97,0.99], 
Target  Position  [389.90,-3.0,103.9],  Obstacle  initial  position  [159.9,-1.5,62.9],  Obstacle 
final  position  [199.0,-1.5,62.98]  and  UAV  Final  location  [386.7,-2.4,103.3].  Point  mass 
model  of  UAV  is  considered  and  unscented  kalman  filter  is  used  for  estimation.  Fig.  5.35 
shows  the  trajectory  of  the  UAV.  The  trajectory  plot  is  obtained  through  actual  guidance 
as  well  as  guidance  through  autopilot  compensation.  The  guidance  commands  for  the  UAV 
with  point  mass  model  is  shown  in  Fig.  5.45  where  actual  guidance  command  is  tracking 
the  desired  guidance  command. 

The  physical  guidance  command  is  shown  in  Fig.  5.46  with  actual  and  commanded  values 
of  a,  /i  and  thrust.  The  autopilot  compensation  for  physical  guidance  command  is  shown  in 
Fig.  5.38. 
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UAV  Estimated  Trajectory 


Figure  5.35:  UAV  Point  mass  model  with  UKF  Single  Moving  Obstacle:  UAV  Trajectory 
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Figure  5.36:  UAV  Point  mass  model  with  UKF  Figure  5.37:  UAV  Point  mass  model  with  UKF 
Single  Moving  Obstacle:  7  Velocity  Single  Moving  Obstacle:  //,  a  and  Thrust 
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Figure  5.38:  UAV  Point  mass  model  with  UKF 
Single  Moving  Obstacle:  Autopilot  Compensa¬ 
tion 


Figure  5.39:  UAV  Point  mass  model  with  UKF 
Single  Moving  Obstacle:  UAV  to  Obstacle  and 
Target  Distance 


The  UAV  to  target  distance  and  the  UAV  to  obstacle  distance  profiles  are  shown  in  Fig. 
5.39. 

The  obstacle  is  assumed  to  be  moving  with  constant  velocity  and  based  on  successive 
data  obtained  from  camera  sensors  in  their  respective  coordinate  frames,  using  sensor  state 
dynamics  and  using  the  unscented  kalman  filter  estimation,  the  velocity  of  the  obstacle  is 
estimated  and  as  shown  in  the  Fig.5.40.  The  velocity  estimate  becomes  erroneous  as  the 
camera  sensor  comes  to  closer  to  obstacle.  This  can  be  correlated  with  the  time  instants 
shown  in  the  Fig. 5. 41  where  the  forward  direction(x-direction)  position  components  of  UAV 
and  obstacle  are  plotted. 
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Obstacle  True  and  Estimated  Velocity  (m/s) 
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Figure  5.40:  UAV  Point  mass  model  with  UKF 
Single  Moving  Obstacle:  Obstacle  True  and  Es¬ 
timated  Velocity 
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Figure  5.41:  UAV  Point  mass  model  with  UKF 
Single  Moving  Obstacle:  Obstacle  and  UAV  Po¬ 
sition  in  x-direction 
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Figure  5.42:  UAV  Point  mass  model  with  UKF  Single  Moving  Obstacle:  Sigma  error  bound 
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The  sigma  error  bound  for  stereovision  camera  sensor  states  is  shown  in  Fig.  5.42.  The 
camera  sensor  states  are  well  with  in  bounds. 
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UAV  Estimated  Trajectory 
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Figure  5.43:  UAV  Point  mass  model  with  UKF  Multi  Stationary  Obstacle:  UAV  Trajectory 

5.5.2  Multiple  obstacle 

In  order  to  test  the  performance  of  reactive  collision  avoidance  algorithm  in  multiple  sta¬ 
tionary  obstacle  scenario,  the  simulation  is  carried  out  with  three  stationary  obstacles  with 
Initial  positions  [89. 9, -2. 5, 94. 4], [179. 9, 7. 48, 99. 49]  and  [299.9,12.48,101.49]  the  initial  posi¬ 
tion  of  UAV  is  [0,0,50],  Initial  Velocity  of  UAV  [19.95,0.97,0.99],  Target  Position  [359.9,- 
4.5,102.45],  UAV  final  position  [356.23,-4.8,100.68].  point  mass  model  of  UAV  is  considered 
with  UKF  Estimation.  Fig.  5.43  shows  the  trajectory  of  the  UAV  obtained  through  actual 
guidance  command  as  well  as  guidance  with  autopilot  compensation.  In  order  to  provide  a 
clear  view  of  UAV’s  closest  approach  with  the  three  obstacles,  the  UAV  to  target  distance 
profile  and  the  UAV  to  three  different  obstacles  distance  profile  is  shown  in  Fig.  5.44.  The 
guidance  commands  for  the  UAV  with  point  mass  model  is  shown  in  fig.  5.45  with  actual 
and  commanded  guidance  profiles  for  Xil  and  velocity. 

The  physical  guidance  commands  a,  fi  and  thrust  actual  and  commanded  profiles  is 
shown  in  Fig.  5.46.  The  autopilot  compensation  for  physical  guidance  command  is  shown 
in  Fig.  5.47. 
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Distance  from  UAV  to  Obstacles  and  Target 


Figure  5.44:  UAV  Point  mass  model  with  UKF  Multi  Stationary  Obstacle:  UAV  to  Obstacles 
and  Target  Distance 
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Figure  5.45:  UAV  Point  mass  model  with  UKF  Multi  Stationary  Obstacle:  y,  7  and  Velocity 
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Figure  5.46:  UAV  Point  mass  model  with  UKF 
Multi  Stationary  Obstacle:  fi,  a  and  Thrust 
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Figure  5.47:  UAV  Point  mass  model  with  UKF 
Multi  Stationary  Obstacle:  Autopilot  Compen¬ 
sation 


It  is  observed  that  the  guidance  command  is  efficiently  able  to  perform  the  reactive 
collision  avoidance  for  single  moving  obstacle  as  well  as  multiple  stationary  obstacles  scenario 
in  the  noisy  sensor  environments  with  UKF  estimation  of  camera  sensor  states. 

5.5.3  Randomized  Validation 

In  order  to  test  the  performance  of  the  reactive  collision  avoidance  algorithm  in  different 
randomized  initial  conditions,  simulations  are  carried  out  for  single  moving  obstacle.  Point 
mass  model  of  UAV  is  used  for  system  dynamics  and  UKF  is  used  for  camera  sensor  state 
estimation.  The  parameters  randomly  chosen  are  UAV  initial  velocity  and  initial  position, 
obstacle  initial  position  and  destination.  1000  randomized  simulation  runs  are  carried  out. 
The  obstacle  position  is  estimated  using  UKF  and  the  effectiveness  of  nonlinear  DGG  guid¬ 
ance  law  is  tested.  Fig.  (5.48)  shows  the  success  criteria  of  proposed  algorithm  in  different 
success  bands  SI,  S2,  S3  and  S4.  The  percentage  of  success  for  different  bands  are  tabulated 
in  Table  5.1. 


Table  5.1:  Success  Percentage  with  Obstacle  Estimation 


Success 

Band 

Tolerable  safety  sphere 
violation  (as  %  of 
the  safety  sphere  radius) 

Success 

percentage 

SI -Band 

10  % 

86  % 

S2-Band 

20  % 

99.2  % 

S3-Band 

30  % 

100  % 

S4-Band 

40  % 

100  % 

Validation  results  show  that  reactive  collision  avoidance  algorithm  is  quite  effective  and 
reliable. 
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Figure  5.48:  UAVs  Closest  Approach  to  Obstacle  for  1000  runs 
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Chapter  7 
Conclusion 


UAVs  are  playing  a  vital  role  in  numerous  applications.  Even  in  the  areas  which  are  inac¬ 
cessible  to  human  beings,  UAVs  can  outperform  with  onboard  vision  sensors  and  in-built 
processors.  The  present  work  focuses  on  the  reactive  obstacle  avoidance  problem  for  un¬ 
accountable  stationary  obstacles  like  urban  edifices,  poles  as  well  as  moving  obstacles  like 
another  UAV,  flying  birds  etc. 

The  guidance  strategy  applied  in  the  kinematic  model  as  well  as  point  mass  model  based 
formulation  uses  the  collision  cone  approach  for  obstacle  detection  and  executes  the  avoidance 
maneuver  by  generating  the  angular  guidance  commands  in  the  horizontal  and  the  vertical 
planes.  UAV  pursues  the  guidance  commands  by  quickly  aligning  its  velocity  vector  along 
the  aiming  point  while  enforcing  the  turn  coordination  in  the  case  of  point  mass  model.  The 
obstacle  position  as  well  as  velocity  can  be  considered  to  be  partially  known  with  some  noise 
and  hence  can  be  estimated  by  using  the  EKF  and  UKF  techniques.  In  all  the  simulations, 
all  the  constraints  posed  by  the  vehicle  capability  are  very  well  met  within  the  available 
time-to-go. 

The  proposed  Formulation  can  be  enhanced  by  augmenting  it  with  features  like  introduc¬ 
ing  moving  obstacles  with  specific  maneuvers  along  with  the  stationary  obstacles.  Instead 
of  spherical  safety  zones  around  the  obstacles,  more  optimized  shape  of  the  safety  zone  like 
cylinder  can  be  considered  to  accommodate  obstacles  like  electric  poles  and  wires.  The  ob¬ 
stacle  information  can  also  be  processed  through  real  passive  sensors  like  cameras  which  may 
involve  wide  exploration  in  the  computer  vision  techniques. 

Reactive  maneuvers  for  obstacle  avoidance  demands  guidance  and  control  to  execute  in 
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synergy.  Test  cases  for  the  point  mass  model  with  a  coordinated  flight  and  kinematic  model 
is  demonstrated  with  all  scenarios,  where  the  controller  dynamics  were  approximated  as  the 
first  order  autopilots.  Using  the  stereovision  camera,  an  effective  reactive  collision  avoidance 
algorithm  is  presented  in  this  report.  The  following  assumptions  are  made,  (i)  the  shape  and 
size  of  the  obstacle  is  known  and  (ii)  the  data  collected  from  the  vision  sensors  are  assumed  to 
be  noisy.  So  the  camera  captured  position  information  may  become  noisy,  to  overcome  this, 
two  standard  filtering  techniques  such  as  Extended  Kalman  Filter(EKF)  and  Unscented 
Kalman  Filter(UKF)  are  proposed  to  extract  the  useful  information  about  the  obstacle 
position  from  the  noisy  camera  data.  Obstacle  velocity  information  is  also  derived  from  the 
second  order  sensor  dynamics  and  estimated.  However  in  later  portion  of  the  report,  virtual 
simulation  is  carried  out  for  spherical  obstacle  by  extracting  the  potential  feature  points  from 
stereo  images  using  harris  detector  and  then,  stereo  matching  is  done  along  the  epipolor  line. 
Finally,  the  3D  information  of  obstacle  obtained  by  applying  triangulation  method  to  the 
matched  feature  points.  Next,  following  the  collision  cone  approach,  an  ‘aiming  point’  is 
computed  and  nonlinear  differential  geometric  guidance  is  applied  to  steer  away  the  vehicle 
quickly  from  the  impending  danger.  Simulation  studies  leads  to  the  conclusion  that  this 
strategy  is  quite  effective  in  avoiding  popup  obstacles  within  a  very  short  time  and  hence 
can  be  very  useful  for  reactive  collision  avoidance.  This  algorithm  generalized  to  include  (i) 
moving  obstacles  with  simple  maneuvers,  (ii)  more  than  one  obstacle  at  the  same  time  and 
(iii)  appropriate  guidance  of  the  vehicle  after  reaching  the  aiming  point. 
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Appendix  A 

Mathematical  Model 


In  this  report,  two  standard  system  dynamics  for  the  vehicle  movement  have  been  considered. 
First,  a  simplistic  ’’kinematic  model”  has  been  considered  with  autopilot  lags  in  all  channels 
of  the  state  equation  for  quick  validation  of  the  basic  philosophy.  Subsequently,  to  be  more 
realistic,  the  proposed  guidance  strategy  has  been  reworked  using  a  ’’point  mass  model”  of 
the  vehicle  and  further  algebra  has  been  carried  out  by  introducing  an  additional  inner  loop 
to  generate  the  physically  meaningful  guidance  parameters,  namely  the  desired  bank  angle, 
angle  of  attack  and  the  thrust  required  for  the  vehicle,  as  the  necessary  guidance  parameters. 
Note  that  to  be  even  more  realistic,  autopilot  lags  for  these  quantities  have  been  assumed  as 
well  while  validating  our  proposed  guidance  strategy,  which  will  be  discussed  later.  In  this 
section,  the  relevant  details  of  the  mathematical  models  of  both  kinematic  as  well  as  point 
mass  models  used  in  this  research  are  given  in  fair  detail. 
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A.l  Kinematic  Model 


The  relevant  set  of  equations  in  this  model  are  given  by 


X 

=  Vr  cos  7  cos  7 

(A.l) 

y 

=  Vr  sin  7  cos  7 

(A.2) 

h 

=  Vr  sin  7 

(A.3) 

vT 

=  kv(v£-vT) 

(A.4) 

7 

=  M7c~7) 

(A.5) 

X 

=  kx(xc~x) 

(A.6) 

where,  Vr  is  the  velocity  of  the  UAV  whose  directions  are  given  by  two  angles,  namely  the 
flight  path  angle  7  and  course  angle  7.  The  first  three  equations  of  Eq.(A.l-A.3)  represent  the 
movement  of  the  centre  of  mass  of  the  vehicle  in  an  inertial  frame  depending  on  the  velocity 
vector  magnitude  and  its  orientation.  The  next  three  equations  essentially  represent  first 
order  lags,  which  in  turn  govern  the  evolution  of  Vr,  7  and  7,  based  on  their  commanded 
values  V£,  7C  and  xc  respectively.  The  key  idea  here  is  to  generate  yc  and  x°  from  an 
appropriate  guidance  formulation,  as  well  as  giving  an  appropriate  command,  so  that  the 
obstacles  can  be  avoided. 


A. 2  Point  Mass  Model 

To  achieve  obstacle  avoidance  in  a  more  realistic  environment,  the  vehicle  is  considered  as 
a  point  mass  object.  Vp  is  the  inertial  velocity  of  the  UAV  whose  directions  are  given  by 
two  angles,  course  angle(y)  and  flight  path  angle(7  ).  Hence,  the  angular  corrections  on 
the  point  mass  are  realized  by  the  basic  aerodynamic  forces  acting  on  the  vehicle.  The  6th 
order  model  representing  the  coordinated,  symmetric  flight  over  a  flat  earth  (with  standard 
meaning  of  the  variables  [20])  is  considered.  In  the  coordinated  flight,  the  side  force  acting 
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on  the  vehicle  is  null  and  hence  the  side  slip  angle  (3  =  0  [32], 


X  = 

Vt  cos  x  cos  7 

(A.7) 

y  = 

Vt  sin  x  cos  7 

(A.8) 

h  = 

Vt  sin  7 

(A.9) 

x  = 

— — - (T  sin  a  +  L)  sin  // 

mVr  cos  7 

(A. 10) 

7  = 

- \(T  sin  a  +  L)  cos  g  —  mg  cos  7] 

mVr 

(A.ll) 

VT  = 

—  (T  cos  a  —  D  —  mg  sin  7) 

(A. 12) 

Therefore,  the  only  forces  acting  on  the  vehicle  are  thrust,  lift,  drag  and  weight.  To  model  the 
aerodynamic  forces,  aerodynamic  data  of  a  prototype  UAV,  named  as  all  electric  airplane-2 
(AE  —  2)  [39]  is  used. 


Figure  A.l:  AE-2  (Picture  of  All  Electric  airplane-2) 

This  UAV  has  a  reference  area  of  S'  =  0.6m2  and  it  has  mass  Qkg.  Maximum  value  of 
thrust  ( Tmax )  which  can  be  produced  by  its  electric  motor  and  propeller  assembly  is  15V. 
It  is  assumed  that  thrust  produced  has  linear  relation  with  the  throttle  input.  Forces  L  and 
D  acting  on  the  vehicle  are  given  by 

L  =  qSCL  D  =  qSCD  (A.  13) 

q  is  the  dynamic  pressure, Cx  and  Co  are  the  lift  and  drag  coefficients  in  wind  axes  system 
which  can  be  expressed  in  terms  of  axial  and  normal  force  coefficients  Cx  and  Cz  of  the 
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body  axes  system. 


(A. 14) 

Xa,  Za  are  the  body  axes  accelerations,  Lw/b  is  the  transformation  matrix  from  body  to 
wind  axes  system  and  is  given  by 


D 

-L 


=  rriL 


W/B 


Xn 


Lw/b 


cos  a  sin  a 
—  sin  a  cos  a 


Note  that  Eq.  (A.  14)  assumes  that  the  side-slip  angle  is  zero.  This  is  very  close  to 
reality  with  the  assumption  of  co-ordinated  turn,  which  is  ensured  in  the  formulation  while 
generating  the  necessary  bank  angle  as  the  set  of  equations  in  Eq.  (A.  10- A.  12)  are  valid  only 
under  this  assumption.  By  expanding  Eq.  (A.  14),  drag  and  lift  forces  can  be  expressed  in 
terms  of  body  axes  axial  and  normal  force  coefficients  as  follows 


D  =  -qS  [( CXq  +  CXalOi  +  CXa2a 2)  cos  a  +  ( CZo  +  CZalQt)  sin  a] 

L  =  —qS  [(Cx0  +  Cxal &  +  Cxa2a~)(—  sin  a)  +  ( Cz0  +  Czalot )  cos  a:] 

Where,  Cx0  and  Cz0  are  the  aerodynamic  coefficients  and  Czal,Cxal,Cxa2  are  the  dynamic 
derivatives  of  the  aerodynamic  coefficients.  The  initial  values  of  the  aerodynamic  coefficients 
and  their  derivatives  corresponding  to  the  UAV  model  [39]  are  given  in  Table  A.l. 


Table  A.l:  Initial  Value  of  Coefficients 


L'Xq 

L'Zo 

cZal 

Cxal 

C'Xa2 

0.0386 

0.1653 

0.087138 

-0.0040376 

-0.0010525 

Note  that  the  ultimate  aim  here  is  to  ford  the  desired  values  of  angle  of  attack  a  ,  bank 
angle  a  and  the  desired  thrust  T  that  will  ensure  collision  avoidance. 

First  order  autopilots  are  designed  for  all  the  control  variables  which  can  be  stated  as 
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/j,*,  a*,  T*  are  the  control  variables  with  full  bandwidth  and  the  fid,  &d,  Td  are  the  control 
variables  obtained  after  the  first  order  delay  of  the  autopilot.  Gains  for  autopilot  are  much 
higher  than  the  gains  for  the  guidance  loop  due  to  difference  in  the  settling  time  of  the 
guidance  and  autopilot  dynamics.  The  position  limits  on  the  control  variables  /j,d,  ad,  Td 
are  given  by  fid  <  ±45°  (maximum  turn  angle),  ad  <  ±10°  (stall  angle)  and  Td  <  ±15° 
(maximum  thrust  limit). 
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